From 193d54a0a650b1ec871e9c216c77d00642ffaba8 Mon Sep 17 00:00:00 2001
From: topquark12 <aw@m-labs.hk>
Date: Mon, 11 Jan 2021 14:00:52 +0800
Subject: [PATCH] pid: anti-windup when compliance voltage reached

---
 src/channel_state.rs |  6 ++++--
 src/channels.rs      |  4 ++--
 src/pid.rs           | 16 ++++++++++++----
 3 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/src/channel_state.rs b/src/channel_state.rs
index faa7ae8..b93dd75 100644
--- a/src/channel_state.rs
+++ b/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)
     }
 
diff --git a/src/channels.rs b/src/channels.rs
index ccdf8f7..037a8b2 100644
--- a/src/channels.rs
+++ b/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));
diff --git a/src/pid.rs b/src/pid.rs
index 74ba37f..b2a3f61 100644
--- a/src/pid.rs
+++ b/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;