rewrite PID

Rewrite of PID according to https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation.

To migrate:
- TEC+/- pin polarity has to be reversed.
- Some saved settings might be wiped upon flashing of new firmware, back up settings before upgrade
- Min / Max integral parameters no longer exist
- kp, ki, kd will likely need to be retuned

The software has been tested on hardware with good temperature control stability.

Reviewed-on: #68
Co-authored-by: topquark12 <aw@m-labs.hk>
Co-committed-by: topquark12 <aw@m-labs.hk>

README.md

@@ -114,8 +114,6 @@ formatted as line-delimited JSON.
 | `pid <0/1> kd <value>`           | Set differential gain                                                |
 | `pid <0/1> output_min <amp>`     | Set mininum output                                                   |
 | `pid <0/1> output_max <amp>`     | Set maximum output                                                   |
-| `pid <0/1> integral_min <value>` | Set integral lower bound                                             |
-| `pid <0/1> integral_max <value>` | Set integral upper bound                                             |
 | `s-h`                            | Show Steinhart-Hart equation parameters                              |
 | `s-h <0/1> <t0/b/r0> <value>`    | Set Steinhart-Hart parameter for a channel                           |
 | `postfilter`                     | Show postfilter settings                                             |

pytec/autotune.py

@@ -114,9 +114,9 @@ class PIDAutotune:
 
         # set output
         if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP):
-            self._output = self._initial_output + self._outputstep
-        elif self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN:
             self._output = self._initial_output - self._outputstep
+        elif self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN:
+            self._output = self._initial_output + self._outputstep
 
         # respect output limits
         self._output = min(self._output, self._out_max)
@@ -223,7 +223,7 @@ def main():
     # Thermostat channel
     channel = 0
     # Target temperature of the autotune routine, celcius
-    target_temperature = 30
+    target_temperature = 20
     # Value by which output will be increased/decreased from zero, amps
     output_step = 1
     # Reference period for local minima/maxima, seconds

pytec/pytec/client.py

@@ -67,22 +67,16 @@ class Client:
                   'ki': 0.02,
                   'kd': 0.0,
                   'output_min': 0.0,
-                  'output_max': 3.0,
+                  'output_max': 3.0},
-                  'integral_min': -100.0,
+              'target': 37.0},
-                  'integral_max': 100.0},
-              'target': 37.0,
-              'integral': 38.41138597026372},
              {'channel': 1,
               'parameters': {
                   'kp': 10.0,
                   'ki': 0.02,
                   'kd': 0.0,
                   'output_min': 0.0,
-                  'output_max': 3.0,
+                  'output_max': 3.0},
-                  'integral_min': -100.0,
+              'target': 36.5}]
-                  'integral_max': 100.0},
-              'target': 36.5,
-              'integral': nan}]
         """
         return self._get_conf("pid")
 

src/channel_state.rs

@@ -3,7 +3,6 @@ use uom::si::{
     f64::{
         ElectricPotential,
         ElectricalResistance,
-        ElectricCurrent,
         ThermodynamicTemperature,
         Time,
     },
@@ -67,10 +66,10 @@ impl ChannelState {
     }
 
     /// Update PID state on ADC input, calculate new DAC output
-    pub fn update_pid(&mut self, current: ElectricCurrent) -> Option<f64> {
+    pub fn update_pid(&mut self) -> Option<f64> {
         let temperature = self.get_temperature()?
             .get::<degree_celsius>();
-        let pid_output = self.pid.update(temperature, self.get_adc_interval(), current);
+        let pid_output = self.pid.update(temperature);
         Some(pid_output)
     }
 

src/channels.rs

@@ -75,10 +75,9 @@ impl Channels {
     pub fn poll_adc(&mut self, instant: Instant) -> Option<u8> {
         self.adc.data_ready().unwrap().map(|channel| {
             let data = self.adc.read_data().unwrap();
-            let current = self.get_tec_i(channel.into());
             let state = self.channel_state(channel);
             state.update(instant, data);
-            match state.update_pid(current) {
+            match state.update_pid() {
                 Some(pid_output) if state.pid_engaged => {
                     // Forward PID output to i_set DAC
                     self.set_i(channel.into(), ElectricCurrent::new::<ampere>(pid_output));
@@ -437,9 +436,7 @@ impl Channels {
         let tec_i = self.get_tec_i(channel);
         let dac_value = self.get_dac(channel);
         let state = self.channel_state(channel);
-        let pid_output = state.pid.last_output.map(|last_output|
+        let pid_output = ElectricCurrent::new::<ampere>(state.pid.y1);
-            ElectricCurrent::new::<ampere>(last_output)
-        );
         Report {
             channel,
             time: state.get_adc_time(),
@@ -541,7 +538,7 @@ pub struct Report {
     i_tec: ElectricPotential,
     tec_i: ElectricCurrent,
     tec_u_meas: ElectricPotential,
-    pid_output: Option<ElectricCurrent>,
+    pid_output: ElectricCurrent,
 }
 
 pub struct CenterPointJson(CenterPoint);

src/command_handler.rs

@@ -231,10 +231,6 @@ impl Handler {
                 pid.parameters.output_min = value as f32,
             OutputMax =>
                 pid.parameters.output_max = value as f32,
-            IntegralMin =>
-                pid.parameters.integral_min = value as f32,
-            IntegralMax =>
-                pid.parameters.integral_max = value as f32,
         }
         send_line(socket, b"{}");
         Ok(Handler::Handled)

src/command_parser.rs

@@ -111,8 +111,6 @@ pub enum PidParameter {
     KD,
     OutputMin,
     OutputMax,
-    IntegralMin,
-    IntegralMax,
 }
 
 /// Steinhart-Hart equation parameter
@@ -369,8 +367,6 @@ fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
              value(PidParameter::KD, tag("kd")),
              value(PidParameter::OutputMin, tag("output_min")),
              value(PidParameter::OutputMax, tag("output_max")),
-             value(PidParameter::IntegralMin, tag("integral_min")),
-             value(PidParameter::IntegralMax, tag("integral_max"))
         ))(input)?;
     let (input, _) = whitespace(input)?;
     let (input, value) = float(input)?;
@@ -701,16 +697,6 @@ mod test {
         }));
     }
 
-    #[test]
-    fn parse_pid_integral_max() {
-        let command = Command::parse(b"pid 1 integral_max 2000");
-        assert_eq!(command, Ok(Command::Pid {
-            channel: 1,
-            parameter: PidParameter::IntegralMax,
-            value: 2000.0,
-        }));
-    }
-
     #[test]
     fn parse_steinhart_hart() {
         let command = Command::parse(b"s-h");

src/pid.rs

@@ -1,12 +1,4 @@
 use serde::{Serialize, Deserialize};
-use uom::si::{
-    f64::{Time, ElectricCurrent},
-    time::second,
-    electric_current::ampere,
-};
-
-/// Allowable current error for integral accumulation
-const CURRENT_ERROR_MAX: f64 = 0.1;
 
 #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
 pub struct Parameters {
@@ -20,10 +12,6 @@ pub struct Parameters {
     pub output_min: f32,
     /// Output limit maximum
     pub output_max: f32,
-    /// Integral clipping minimum
-    pub integral_min: f32,
-    /// Integral clipping maximum
-    pub integral_max: f32
 }
 
 impl Default for Parameters {
@@ -34,8 +22,6 @@ impl Default for Parameters {
             kd: 0.0,
             output_min: -2.0,
             output_max: 2.0,
-            integral_min: -10.0,
-            integral_max: 10.0,
         }
     }
 }
@@ -44,9 +30,10 @@ impl Default for Parameters {
 pub struct Controller {
     pub parameters: Parameters,
     pub target : f64,
-    integral: f64,
+    u1 : f64,
-    last_input: Option<f64>,
+    x1 : f64,
-    pub last_output: Option<f64>,
+    x2 : f64,
+    pub y1 : f64,
 }
 
 impl Controller {
@@ -54,58 +41,38 @@ impl Controller {
         Controller {
             parameters: parameters,
             target : 0.0,
-            last_input: None,
+            u1 : 0.0,
-            integral: 0.0,
+            x1 : 0.0,
-            last_output: None,
+            x2 : 0.0,
+            y1 : 0.0,
         }
     }
 
-    pub fn update(&mut self, input: f64, time_delta: Time, current: ElectricCurrent) -> f64 {
+    // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation
-        let time_delta = time_delta.get::<second>();
+    // Input x(t), target u(t), output y(t)
+    // y0' =   y1 - ki * u0   
+    //       + x0 * (kp + ki + kd)
+    //       - x1 * (kp + 2kd)
+    //       + x2 * kd
+    //       + kp * (u0 - u1)
+    // y0  = clip(y0', ymin, ymax)
+    pub fn update(&mut self, input: f64) -> f64 {
         
-        // error
+        let mut output: f64 = self.y1 - self.target * f64::from(self.parameters.ki)
-        let error = self.target - input;
+                            + input * f64::from(self.parameters.kp + self.parameters.ki + self.parameters.kd)
-
+                            - self.x1 * f64::from(self.parameters.kp + 2.0 * self.parameters.kd)
-        // proportional
+                            + self.x2 * f64::from(self.parameters.kd)
-        let p = f64::from(self.parameters.kp) * error;
+                            + f64::from(self.parameters.kp) * (self.target - self.u1);
-
-        // integral
-        if let Some(last_output_val) = self.last_output {
-            let electric_current_error = ElectricCurrent::new::<ampere>(last_output_val) - current;
-            // anti integral windup
-            if last_output_val < self.parameters.output_max.into() &&
-               last_output_val > self.parameters.output_min.into() &&
-               electric_current_error < ElectricCurrent::new::<ampere>(CURRENT_ERROR_MAX) &&
-               electric_current_error > -ElectricCurrent::new::<ampere>(CURRENT_ERROR_MAX) {
-                self.integral += error * time_delta;    
-            }
-        }
-        if self.integral < self.parameters.integral_min.into() {
-            self.integral = self.parameters.integral_min.into();
-        }
-        if self.integral > self.parameters.integral_max.into() {
-            self.integral = self.parameters.integral_max.into();
-        }
-        let i = self.integral * f64::from(self.parameters.ki);
-
-        // derivative
-        let d = match self.last_input {
-            None =>
-                0.0,
-            Some(last_input) =>
-                f64::from(self.parameters.kd) * (last_input - input) / time_delta,
-        };
-        self.last_input = Some(input);
-
-        // output
-        let mut output = p + i + d;
         if output < self.parameters.output_min.into() {
             output = self.parameters.output_min.into();
         }
         if output > self.parameters.output_max.into() {
             output = self.parameters.output_max.into();
         }
-        self.last_output = Some(output);
+        self.x2 = self.x1;
+        self.x1 = input;
+        self.u1 = self.target;
+        self.y1 = output;        
         output
     }
 
@@ -114,17 +81,10 @@ impl Controller {
             channel,
             parameters: self.parameters.clone(),
             target: self.target,
-            integral: self.integral,
         }
     }
 
     pub fn update_ki(&mut self, new_ki: f32) {
-        if new_ki == 0.0 {
-            self.integral = 0.0;
-        } else {
-            // Rescale integral with changes to kI, aka "Bumpless operation"
-            self.integral = f64::from(self.parameters.ki) * self.integral / f64::from(new_ki);
-        }
         self.parameters.ki = new_ki;
     }
 }
@@ -134,7 +94,6 @@ pub struct Summary {
     channel: usize,
     parameters: Parameters,
     target: f64,
-    integral: f64,
 }
 
 #[cfg(test)]
@@ -147,8 +106,6 @@ mod test {
         kd: 0.15,
         output_min: -10.0,
         output_max: 10.0,
-        integral_min: -1000.0,
-        integral_max: 1000.0,
     };
 
     #[test]
@@ -177,9 +134,9 @@ mod test {
         while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT {
             let next_t = (t + 1) % DELAY;
             // Feed the oldest temperature
-            output = pid.update(values[next_t], Time::new::<second>(1.0), ElectricCurrent::new::<ampere>(output));
+            output = pid.update(values[next_t]);
             // Overwrite oldest with previous temperature - output
-            values[next_t] = values[t] + output - (values[t] - DEFAULT) * LOSS;
+            values[next_t] = values[t] - output - (values[t] - DEFAULT) * LOSS;
             t = next_t;
             total_t += 1;
             println!("{}", values[t].to_string());