Skip to main content

continuous_blocks/
pid.rs

1use crate::state_space::StateSpace;
2use crate::transfer_function::TransferFunction;
3
4/// <!-- doc-ref: pid -->
5/// Parallel PID with a filtered derivative term, using the standard `P, I, D, N` (filter
6/// coefficient) parameterization common to block-diagram continuous-time controller blocks:
7///
8/// $$C(s) = K_p + \frac{K_i}{s} + \frac{K_d N s}{s + N}$$
9///
10/// The pure-derivative term `Kd*s` alone is not realizable as a proper (causal) transfer
11/// function, hence the filter pole at `-N` — a standard, universally-used approximation (a
12/// larger `N` tracks the ideal derivative more closely but stiffens the resulting ODE).
13///
14/// This struct is the compiled gain set only — see `general_mna::block_graph::BlockKind::Pid`
15/// for the netlist-facing component reference (parameters, errors, netlist form, example) and
16/// the anti-windup behavior, which lives at the block-graph evaluation layer, not here.
17#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct Pid {
19    /// Proportional gain.
20    pub kp: f64,
21    /// Integral gain.
22    pub ki: f64,
23    /// Derivative gain.
24    pub kd: f64,
25    /// Derivative filter coefficient; the filter pole sits at `-n`. Must be `> 0` — see
26    /// [`Pid::new`].
27    pub n: f64,
28}
29
30#[derive(Debug, Clone, Copy, PartialEq, Eq)]
31pub enum PidError {
32    /// `n <= 0` puts the derivative filter's pole at `-n`, i.e. at the origin or in the
33    /// right half-plane — a non-causal or outright unstable filter, not the standard,
34    /// universally-used approximation this parameterization exists to give.
35    NonPositiveFilterCoefficient,
36}
37
38impl Pid {
39    pub fn new(kp: f64, ki: f64, kd: f64, n: f64) -> Result<Self, PidError> {
40        if n <= 0.0 {
41            return Err(PidError::NonPositiveFilterCoefficient);
42        }
43        Ok(Pid { kp, ki, kd, n })
44    }
45
46    /// Puts `C(s)` over the common denominator `s*(s+N)`:
47    ///
48    /// ```text
49    /// C(s) = [(Kp+Kd*N)*s^2 + (Kp*N+Ki)*s + Ki*N] / [s^2 + N*s]
50    /// ```
51    ///
52    /// Derived by hand before implementation (see `docs/architecture.md`'s Milestone 5 note):
53    /// with `Kd=0` this numerator factors exactly as `(s+N)(Kp*s+Ki)`, so the filter pole/zero
54    /// cancel and the closed-loop step response of the resulting state-space realization
55    /// reduces to the ideal PI response `y(t) = Kp + Ki*t` for *every* `t`, not just
56    /// asymptotically — `tests/step_responses.rs` checks exactly this.
57    pub fn to_transfer_function(&self) -> TransferFunction {
58        let num = vec![
59            self.kp + self.kd * self.n,
60            self.kp * self.n + self.ki,
61            self.ki * self.n,
62        ];
63        let den = vec![1.0, self.n, 0.0];
64        TransferFunction::new(num, den).expect("PID transfer function is always proper")
65    }
66
67    pub fn to_state_space(&self) -> StateSpace {
68        self.to_transfer_function().to_state_space()
69    }
70}
71
72#[cfg(test)]
73mod tests {
74    use super::*;
75
76    #[test]
77    fn kd_zero_cancels_to_pure_pi_numerator() {
78        // With Kd=0, numerator should equal (s+N)(Kp*s+Ki) expanded, which is what
79        // to_transfer_function already computes directly -- this test instead cross-checks it
80        // against the expanded-by-hand form for a concrete (Kp, Ki, N), as an independent
81        // arithmetic check of the formula itself (not just that the code returns its own
82        // formula back).
83        let pid = Pid::new(2.0, 3.0, 0.0, 10.0).unwrap();
84        let tf = pid.to_transfer_function();
85        // (s+10)(2s+3) = 2s^2 + 3s + 20s + 30 = 2s^2 + 23s + 30
86        assert_eq!(tf.num, vec![2.0, 23.0, 30.0]);
87        assert_eq!(tf.den, vec![1.0, 10.0, 0.0]);
88    }
89
90    #[test]
91    fn non_positive_filter_coefficient_is_rejected_not_silently_unstable() {
92        assert_eq!(
93            Pid::new(2.0, 3.0, 0.5, 0.0),
94            Err(PidError::NonPositiveFilterCoefficient)
95        );
96        assert_eq!(
97            Pid::new(2.0, 3.0, 0.5, -10.0),
98            Err(PidError::NonPositiveFilterCoefficient)
99        );
100    }
101}