src/channel_state.rs

@@ -3,6 +3,7 @@ use uom::si::{
     f64::{
         ElectricPotential,
         ElectricalResistance,
+        ElectricCurrent,
         ThermodynamicTemperature,
         Time,
     },
@@ -66,10 +67,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));
@@ -364,6 +364,16 @@ impl Channels {
         (duty * max, max)
     }
 
+    // Get current passing through TEC
+    pub fn get_tec_i(&mut self, channel: usize) -> ElectricCurrent {
+        (self.read_itec(channel) - self.read_vref(channel)) / ElectricalResistance::new::<ohm>(0.4)
+    }
+
+    // Get voltage across TEC
+    pub fn get_tec_v(&mut self, channel: usize) -> ElectricPotential {
+        (self.read_tec_u_meas(channel) - ElectricPotential::new::<volt>(1.5)) * 4.0
+    }
+
     fn set_pwm(&mut self, channel: usize, pin: PwmPin, duty: f64) -> f64 {
         fn set<P: hal::PwmPin<Duty=u16>>(pin: &mut P, duty: f64) -> f64 {
             let max = pin.get_max_duty();
@@ -417,7 +427,7 @@ impl Channels {
         let vref = self.channel_state(channel).vref;
         let (i_set, _) = self.get_i(channel);
         let i_tec = self.read_itec(channel);
-        let tec_i = (i_tec - vref) / ElectricalResistance::new::<ohm>(0.4);
+        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|
@@ -438,7 +448,7 @@ impl Channels {
             dac_feedback: self.read_dac_feedback(channel),
             i_tec,
             tec_i,
-            tec_u_meas: self.read_tec_u_meas(channel),
+            tec_u_meas: self.get_tec_v(channel),
             pid_output,
         }
     }

src/main.rs

@@ -332,8 +332,8 @@ fn main() -> ! {
                                             pid.target = value,
                                         KP =>
                                             pid.parameters.kp = value as f32,
-                                        KI =>
+                                        KI => 
-                                            pid.parameters.ki = value as f32,
+                                            pid.update_ki(value as f32),
                                         KD =>
                                             pid.parameters.kd = value as f32,
                                         OutputMin =>

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
@@ -25,10 +29,10 @@ pub struct Parameters {
 impl Default for Parameters {
     fn default() -> Self {
         Parameters {
-            kp: 1.5,
+            kp: 0.0,
-            ki: 1.0,
+            ki: 0.0,
-            kd: 1.5,
+            kd: 0.0,
-            output_min: 0.0,
+            output_min: -2.0,
             output_max: 2.0,
             integral_min: -10.0,
             integral_max: 10.0,
@@ -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;    
             }
         }
@@ -109,6 +117,16 @@ impl Controller {
             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;
+    }
 }
 
 #[derive(Clone, Serialize, Deserialize)]
@@ -158,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;