A simple PID controller

pid.rs

//! A simple PID controller

use std::f32;

pub struct Pid {
    kp: f32,
    ki: f32,
    kd: f32,
    setpoint: f32,
    min: f32,
    max: f32,
    prev_value: f32,
    integral: f32,
}

impl Pid {
    /// Create new "ideal" PID controller with given Kp, Ki and Kd gains.
    /// Kp = proportional gain, Ki = integral gain, Kd = derivative gain
    pub fn new(kp: f32, ki: f32, kd: f32) -> Pid {
        Pid {
            kp: kp,
            ki: ki,
            kd: kd,
            setpoint: 0.0,
            min: -f32::INFINITY,
            max: f32::INFINITY,
            prev_value: 0.0,
            integral: 0.0,
        }
    }

    /// Create new "standard" PID controller with given Kp gain, Ti and Td times.
    /// Kp = proportional gain, Ti = integral time, Td = derivative time
    pub fn new_std(kp: f32, ti: f32, td: f32) -> Pid {
        Self::new(kp, kp / ti, kp * td)
    }

    /// Create new PID controller tuned by Ziegler-Nichols method using Ku gain and Tu time.
    /// Ku = ultimative gain (Kp where output starts to oscillate), Tu = oscillation period
    pub fn new_zn(ku: f32, tu: f32) -> Pid {
        Self::new_std(0.6 * ku, 0.5 * tu, 0.125 * tu)
    }

    pub fn set_limits(&mut self, min: f32, max: f32) {
        self.min = min;
        self.max = max;
    }

    pub fn set(&mut self, setpoint: f32) {
        self.setpoint = setpoint;
    }

    pub fn update(&mut self, value: f32, dt: f32) -> f32 {
        let error = self.setpoint - value;
        let proportional = error * self.kp;
        self.integral += error * dt * self.ki;                      // Sum up weighted integral and
        self.integral = self.integral.max(self.min).min(self.max);  // apply min/max bounds to prevent "integral windup".
        let derivative = -(value - self.prev_value) / dt * self.kd; // Use neg. delta value instead error for derivative to
        self.prev_value = value;                                    // prevent "derivative kicks" when changing setpoint.
        (proportional + self.integral + derivative).max(self.min).min(self.max)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn pure_p_controller() {
        let mut c = Pid::new(0.5, 0.0, 0.0);
        c.set(10.0);
        assert_eq!(c.update(  5.0, 1.0),    2.5);
        assert_eq!(c.update( 10.0, 1.0),    0.0);
        assert_eq!(c.update( 15.0, 1.0),   -2.5);
        assert_eq!(c.update(100.0, 1.0),  -45.0);
        c.set(1.0);
        assert_eq!(c.update(  0.0, 1.0),    0.5);
    }

    #[test]
    fn pure_i_controller() {
        let mut c = Pid::new(0.0, 0.5, 0.0);
        c.set(10.0);
        assert_eq!(c.update(  5.0, 1.0),    2.5);
        assert_eq!(c.update(  5.0, 1.0),    5.0);
        assert_eq!(c.update(  5.0, 1.0),    7.5);
        assert_eq!(c.update( 15.0, 1.0),    5.0);
        assert_eq!(c.update( 20.0, 1.0),    0.0);
    }

    #[test]
    fn pure_d_controller() {
        let mut c = Pid::new(0.0, 0.0, 0.5);
        c.set(10.0);
        assert_eq!(c.update(  5.0, 1.0),   -2.5);
        assert_eq!(c.update(  5.0, 1.0),    0.0);
        assert_eq!(c.update( 10.0, 1.0),   -2.5);
    }

    #[test]
    fn pid_controller() {
        let mut c = Pid::new(0.5, 0.5, 0.5);
        c.set(10.0);
        assert_eq!(c.update(  5.0, 1.0),    2.5);
        assert_eq!(c.update( 10.0, 1.0),    0.0);
        assert_eq!(c.update( 15.0, 1.0),   -5.0);
        assert_eq!(c.update(100.0, 1.0), -132.5);
        c.set(1.0);
        assert_eq!(c.update(  0.0, 1.0),    6.0);
    }

    #[test]
    fn thermostat_example() {
        let mut c = Pid::new(0.6, 1.2, 0.075);
        c.set_limits(0.0, 1.0);
        c.set(72.0);
        assert_eq!(c.update( 50.0, 1.0),    1.0);
        assert_eq!(c.update( 51.0, 1.0),    1.0);
        assert_eq!(c.update( 55.0, 1.0),    1.0);
        assert_eq!(c.update( 60.0, 1.0),    1.0);
        assert_eq!(c.update( 75.0, 1.0),    0.0);
        assert_eq!(c.update( 76.0, 1.0),    0.0);
        assert_eq!(c.update( 74.0, 1.0),    0.0);
        assert_eq!(c.update( 72.0, 1.0),    0.15);
        assert_eq!(c.update( 71.0, 1.0),    1.0);
    }

    #[test]
    fn pd_controller() {
        let mut c = Pid::new(40.0, 0.0, 12.0);
        c.set_limits(0.0, 255.0);
        c.set(90.0);
        assert_eq!(c.update( 22.00, 1.0),  255.0);
        assert_eq!(c.update( 25.29, 1.0),  255.0);
        assert_eq!(c.update( 28.56, 1.0),  255.0);
        assert_eq!(c.update( 31.80, 1.0),  255.0);
        assert_eq!(c.update( 35.02, 1.0),  255.0);
        assert_eq!(c.update( 38.21, 1.0),  255.0);
        assert_eq!(c.update( 41.38, 1.0),  255.0);
        assert_eq!(c.update( 44.53, 1.0),  255.0);
        assert_eq!(c.update( 47.66, 1.0),  255.0);
        assert_eq!(c.update( 50.76, 1.0),  255.0);
        assert_eq!(c.update( 53.84, 1.0),  255.0);
        assert_eq!(c.update( 56.90, 1.0),  255.0);
        assert_eq!(c.update( 59.93, 1.0),  255.0);
        assert_eq!(c.update( 62.95, 1.0),  255.0);
        assert_eq!(c.update( 65.94, 1.0),  255.0);
        assert_eq!(c.update( 68.91, 1.0),  255.0);
        assert_eq!(c.update( 71.85, 1.0),  255.0);
        assert_eq!(c.update( 74.78, 1.0),  255.0);
        assert_eq!(c.update( 77.69, 1.0),  255.0);
        assert_eq!(c.update( 80.57, 1.0),  255.0);
        assert_eq!(c.update( 83.43, 1.0),  228.47998);
        assert_eq!(c.update( 85.93, 1.0),  132.79999);
        assert_eq!(c.update( 87.18, 1.0),   97.79999);
        assert_eq!(c.update( 87.96, 1.0),   72.24005);
        assert_eq!(c.update( 88.41, 1.0),   58.1998);
        assert_eq!(c.update( 88.68, 1.0),   49.560028);
        assert_eq!(c.update( 88.83, 1.0),   44.99991);
        assert_eq!(c.update( 88.92, 1.0),   42.120117);
        assert_eq!(c.update( 88.98, 1.0),   40.079803);
        assert_eq!(c.update( 89.00, 1.0),   39.76004);
        assert_eq!(c.update( 89.03, 1.0),   38.440063);
        assert_eq!(c.update( 89.03, 1.0),   38.80005);
        assert_eq!(c.update( 89.05, 1.0),   37.759827);
        assert_eq!(c.update( 89.04, 1.0),   38.51999);
        assert_eq!(c.update( 89.05, 1.0),   37.879852);
        assert_eq!(c.update( 89.05, 1.0),   37.999878);
        assert_eq!(c.update( 89.05, 1.0),   37.999878);
        assert_eq!(c.update( 89.05, 1.0),   37.999878);
        assert_eq!(c.update( 89.05, 1.0),   37.999878);
    }
}