use crate::state_space::StateSpace;
use crate::transfer_function::TransferFunction;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Pid {
pub kp: f64,
pub ki: f64,
pub kd: f64,
pub n: f64,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum PidError {
NonPositiveFilterCoefficient,
}
impl Pid {
pub fn new(kp: f64, ki: f64, kd: f64, n: f64) -> Result<Self, PidError> {
if n <= 0.0 {
return Err(PidError::NonPositiveFilterCoefficient);
}
Ok(Pid { kp, ki, kd, n })
}
pub fn to_transfer_function(&self) -> TransferFunction {
let num = vec![
self.kp + self.kd * self.n,
self.kp * self.n + self.ki,
self.ki * self.n,
];
let den = vec![1.0, self.n, 0.0];
TransferFunction::new(num, den).expect("PID transfer function is always proper")
}
pub fn to_state_space(&self) -> StateSpace {
self.to_transfer_function().to_state_space()
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn kd_zero_cancels_to_pure_pi_numerator() {
let pid = Pid::new(2.0, 3.0, 0.0, 10.0).unwrap();
let tf = pid.to_transfer_function();
assert_eq!(tf.num, vec![2.0, 23.0, 30.0]);
assert_eq!(tf.den, vec![1.0, 10.0, 0.0]);
}
#[test]
fn non_positive_filter_coefficient_is_rejected_not_silently_unstable() {
assert_eq!(
Pid::new(2.0, 3.0, 0.5, 0.0),
Err(PidError::NonPositiveFilterCoefficient)
);
assert_eq!(
Pid::new(2.0, 3.0, 0.5, -10.0),
Err(PidError::NonPositiveFilterCoefficient)
);
}
}