automatica 1.0.0

//! # PID (Proportional-integral-derivative) controller
//!
//! Common industrial controllers.
//! * real PID
//! * ideal PID
//! * automatic calculation of the corrisponding transfer function

use std::ops::{Add, Div, Mul};

use crate::{transfer_function::continuous::Tf, One, Zero};

/// Proportional-Integral-Derivative controller
#[derive(Debug)]
pub struct Pid<T> {
    /// Proportional action coefficient
    kp: T,
    /// Integral time
    ti: T,
    /// Derivative time
    td: T,
    /// Constant for additional pole
    n: Option<T>,
}

/// Implementation of Pid methods
impl<T> Pid<T>
where
    T: Add<Output = T> + Clone + Div<Output = T> + Mul<Output = T> + One + PartialEq + Zero,
{
    /// Create a new ideal PID controller
    ///
    /// # Arguments
    ///
    /// * `kp` - Proportional action coefficient
    /// * `ti` - Integral time
    /// * `td` - Derivative time
    ///
    /// # Example
    /// ```
    /// use automatica::controller::pid::Pid;
    /// let pid = Pid::new_ideal(4., 6., 0.1);
    /// ```
    pub fn new_ideal(kp: T, ti: T, td: T) -> Self {
        Self {
            kp,
            ti,
            td,
            n: None,
        }
    }

    /// Create a new real PID controller
    ///
    /// # Arguments
    ///
    /// * `kp` - Proportional action coefficient
    /// * `ti` - Integral time
    /// * `td` - Derivative time
    /// * `n` - Constant for additional pole
    ///
    /// # Example
    /// ```
    /// use automatica::controller::pid::Pid;
    /// let pid = Pid::new(4., 6., 12., 0.1);
    /// ```
    pub fn new(kp: T, ti: T, td: T, n: T) -> Self {
        Self {
            kp,
            ti,
            td,
            n: Some(n),
        }
    }

    /// Calculate the transfer function of the PID controller
    ///
    /// # Real PID
    /// ```text
    ///          1         Td
    /// Kp (1 + ---- + ---------- s) =
    ///         Ti*s   1 + Td/N*s
    ///
    ///      N + (Ti*N +Td)s + Ti*Td(1 + N)s^2
    /// = Kp ----------------------------------
    ///             Ti*N*s + Ti*Td*s^2
    /// ```
    /// # Ideal PID
    ///
    /// ```text
    ///    1 + Ti*s + Ti*Td*s^2
    /// Kp --------------------
    ///           Ti*s
    /// ```
    ///
    /// # Example
    /// ```
    /// use automatica::{controller::pid::Pid, Tf};
    /// let pid = Pid::new_ideal(2., 2., 0.5);
    /// let tf = Tf::new([1., 2., 1.], [0., 1.]);
    /// assert_eq!(tf, pid.tf());
    /// ```
    pub fn tf(&self) -> Tf<T> {
        self.n
            .clone()
            .map_or_else(|| self.tf_from_ideal_pid(), |n| self.tf_from_real_pid(n))
    }

    /// Calculate the transfer function of a real PID controller
    ///
    /// # Arguments
    ///
    /// * `n` - Constant for additional pole
    fn tf_from_real_pid(&self, n: T) -> Tf<T> {
        let a0 = self.kp.clone() * n.clone();
        let a1 = self.kp.clone() * (self.ti.clone() * n.clone() + self.td.clone());
        let a2 = self.kp.clone() * self.ti.clone() * self.td.clone() * (T::one() + n.clone());
        let b0 = T::zero();
        let b1 = self.ti.clone() * n;
        let b2 = self.ti.clone() * self.td.clone();
        Tf::new(&[a0, a1, a2], &[b0, b1, b2])
    }

    /// Calculate the transfer function of an ideal PID controller
    fn tf_from_ideal_pid(&self) -> Tf<T> {
        Tf::new(
            &[T::one(), self.ti.clone(), self.ti.clone() * self.td.clone()],
            &[T::zero(), self.ti.clone() / self.kp.clone()],
        )
    }
}

#[cfg(test)]
mod pid_tests {
    use polynomen::Poly;

    use crate::{units::ToDecibel, Complex};

    use super::*;

    #[test]
    fn ideal_pid_creation() {
        let pid = Pid::new_ideal(10., 5., 2.);
        let tf = pid.tf();
        let c = tf.eval(&Complex::new(0., 1.));
        assert_eq!(Complex::new(10., 18.), c);
    }

    #[test]
    fn real_pid_creation() {
        // Example 15.1
        let g = Tf::new([1.], Poly::new_from_roots(&[-1., -1., -1.]));
        let pid = Pid::new(2.0_f64, 2., 0.5, 5.);
        let r = pid.tf();
        assert_eq!(Some(vec![-10., 0.]), r.real_poles());
        let l = &g * &r;
        let critical_freq = 0.8;
        let c = l.eval(&Complex::new(0., critical_freq));
        assert_abs_diff_eq!(0., c.norm().to_db(), epsilon = 0.1);
    }
}