From b2a58a67033d9a219e3ee486dbc71b221d229aee Mon Sep 17 00:00:00 2001 From: topquark12 Date: Tue, 8 Feb 2022 22:39:55 +0800 Subject: [PATCH] BREAKING, NOT TESTED on hardware, rewrite PID --- src/channels.rs | 6 +-- src/command_handler.rs | 4 -- src/command_parser.rs | 14 ------- src/pid.rs | 95 +++++++++++++----------------------------- 4 files changed, 31 insertions(+), 88 deletions(-) diff --git a/src/channels.rs b/src/channels.rs index 7103034..5119ae7 100644 --- a/src/channels.rs +++ b/src/channels.rs @@ -437,9 +437,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| - ElectricCurrent::new::(last_output) - ); + let pid_output = ElectricCurrent::new::(state.pid.y1); Report { channel, time: state.get_adc_time(), @@ -541,7 +539,7 @@ pub struct Report { i_tec: ElectricPotential, tec_i: ElectricCurrent, tec_u_meas: ElectricPotential, - pid_output: Option, + pid_output: ElectricCurrent, } pub struct CenterPointJson(CenterPoint); diff --git a/src/command_handler.rs b/src/command_handler.rs index aa3b7d1..3144b0c 100644 --- a/src/command_handler.rs +++ b/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) diff --git a/src/command_parser.rs b/src/command_parser.rs index 09fac7b..622f819 100644 --- a/src/command_parser.rs +++ b/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> { 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"); diff --git a/src/pid.rs b/src/pid.rs index 3251d6a..1b0656e 100644 --- a/src/pid.rs +++ b/src/pid.rs @@ -5,8 +5,6 @@ use uom::si::{ 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 +18,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 +28,6 @@ impl Default for Parameters { kd: 0.0, output_min: -2.0, output_max: 2.0, - integral_min: -10.0, - integral_max: 10.0, } } } @@ -43,69 +35,50 @@ impl Default for Parameters { #[derive(Clone)] pub struct Controller { pub parameters: Parameters, - pub target: f64, - integral: f64, - last_input: Option, - pub last_output: Option, + pub target : f64, + u1 : f64, + x1 : f64, + x2 : f64, + pub y1 : f64, } impl Controller { pub const fn new(parameters: Parameters) -> Controller { Controller { parameters: parameters, - target: 0.0, - last_input: None, - integral: 0.0, - last_output: None, + target : 0.0, + u1 : 0.0, + x1 : 0.0, + x2 : 0.0, + y1 : 0.0, } } + // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation + // 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, time_delta: Time, current: ElectricCurrent) -> f64 { - let time_delta = time_delta.get::(); - - // error - let error = self.target - input; - - // proportional - let p = f64::from(self.parameters.kp) * error; - - // 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() && - electric_current_error < ElectricCurrent::new::(CURRENT_ERROR_MAX) && - electric_current_error > -ElectricCurrent::new::(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; + + let mut output: f64 = self.y1 - self.target * f64::from(self.parameters.ki) + + input * f64::from(self.parameters.kp + self.parameters.ki + self.parameters.kd) + - self.x1 * f64::from(self.parameters.kp + 2.0 * self.parameters.kd) + + self.x2 * f64::from(self.parameters.kd) + + f64::from(self.parameters.kp) * (self.target - self.u1); 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 +87,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 +100,6 @@ pub struct Summary { channel: usize, parameters: Parameters, target: f64, - integral: f64, } #[cfg(test)] @@ -147,8 +112,6 @@ mod test { kd: 0.15, output_min: -10.0, output_max: 10.0, - integral_min: -1000.0, - integral_max: 1000.0, }; #[test] @@ -179,7 +142,7 @@ mod test { // Feed the oldest temperature output = pid.update(values[next_t], Time::new::(1.0), ElectricCurrent::new::(output)); // 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());