pid: anti-windup when compliance voltage reached

src/channel_state.rs

@@ -3,11 +3,13 @@ use uom::si::{
     f64::{
         ElectricPotential,
         ElectricalResistance,
+        ElectricCurrent,
         ThermodynamicTemperature,
         Time,
     },
     electric_potential::volt,
     electrical_resistance::ohm,
+    // electric_current::ampere,
     thermodynamic_temperature::degree_celsius,
     time::millisecond,
 };
@@ -66,10 +68,10 @@ impl ChannelState {
     }
 
     /// Update PID state on ADC input, calculate new DAC output
-    pub fn update_pid(&mut self) -> Option<f64> {
+    pub fn update_pid(&mut self, current: ElectricCurrent) -> Option<f64> {
         let temperature = self.get_temperature()?
             .get::<degree_celsius>();
-        let pid_output = self.pid.update(temperature, self.get_adc_interval());
+        let pid_output = self.pid.update(temperature, self.get_adc_interval(), current);
         Some(pid_output)
     }
 

src/channels.rs

@@ -73,10 +73,10 @@ 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() {
+            match state.update_pid(current) {
                 Some(pid_output) if state.pid_engaged => {
                     // Forward PID output to i_set DAC
                     self.set_i(channel.into(), ElectricCurrent::new::<ampere>(pid_output));

src/pid.rs

@@ -1,9 +1,13 @@
 use serde::{Serialize, Deserialize};
 use uom::si::{
-    f64::Time,
+    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 {
     /// Gain coefficient for proportional term
@@ -56,7 +60,7 @@ impl Controller {
         }
     }
 
-    pub fn update(&mut self, input: f64, time_delta: Time) -> f64 {
+    pub fn update(&mut self, input: f64, time_delta: Time, current: ElectricCurrent) -> f64 {
         let time_delta = time_delta.get::<second>();
 
         // error
@@ -67,8 +71,12 @@ impl Controller {
 
         // 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() {
+            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;    
             }
         }
@@ -168,7 +176,7 @@ mod test {
         while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT {
             let next_t = (t + 1) % DELAY;
             // Feed the oldest temperature
-            let output = pid.update(values[next_t], Time::new::<second>(1.0));
+            let output = pid.update(values[next_t], Time::new::<second>(1.0), values[next_t]);
             // Overwrite oldest with previous temperature - output
             values[next_t] = values[t] + output - (values[t] - DEFAULT) * LOSS;
             t = next_t;