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 { + pub fn update_pid(&mut self, current: ElectricCurrent) -> Option { let temperature = self.get_temperature()? .get::(); - 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 { 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::(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::(); // error @@ -67,8 +71,12 @@ impl Controller { // integral if let Some(last_output_val) = self.last_output { + let electric_current_error = ElectricCurrent::new::(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::(CURRENT_ERROR_MAX) && + electric_current_error > -ElectricCurrent::new::(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::(1.0)); + let output = pid.update(values[next_t], Time::new::(1.0), values[next_t]); // Overwrite oldest with previous temperature - output values[next_t] = values[t] + output - (values[t] - DEFAULT) * LOSS; t = next_t;