rewrite PID

Rewrite of PID according to https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation.

To migrate:
- TEC+/- pin polarity has to be reversed.
- Some saved settings might be wiped upon flashing of new firmware, back up settings before upgrade
- Min / Max integral parameters no longer exist
- kp, ki, kd will likely need to be retuned

The software has been tested on hardware with good temperature control stability.

Reviewed-on: M-Labs/thermostat#68
Co-authored-by: topquark12 <aw@m-labs.hk>
Co-committed-by: topquark12 <aw@m-labs.hk>
This commit is contained in:
topquark12 2022-02-24 20:16:47 +08:00 committed by sb10q
parent 69dabf5aa1
commit 26ad2f0119
8 changed files with 43 additions and 116 deletions

View File

@ -114,8 +114,6 @@ formatted as line-delimited JSON.
| `pid <0/1> kd <value>` | Set differential gain | | `pid <0/1> kd <value>` | Set differential gain |
| `pid <0/1> output_min <amp>` | Set mininum output | | `pid <0/1> output_min <amp>` | Set mininum output |
| `pid <0/1> output_max <amp>` | Set maximum output | | `pid <0/1> output_max <amp>` | Set maximum output |
| `pid <0/1> integral_min <value>` | Set integral lower bound |
| `pid <0/1> integral_max <value>` | Set integral upper bound |
| `s-h` | Show Steinhart-Hart equation parameters | | `s-h` | Show Steinhart-Hart equation parameters |
| `s-h <0/1> <t0/b/r0> <value>` | Set Steinhart-Hart parameter for a channel | | `s-h <0/1> <t0/b/r0> <value>` | Set Steinhart-Hart parameter for a channel |
| `postfilter` | Show postfilter settings | | `postfilter` | Show postfilter settings |

View File

@ -114,9 +114,9 @@ class PIDAutotune:
# set output # set output
if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP): if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP):
self._output = self._initial_output + self._outputstep
elif self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN:
self._output = self._initial_output - self._outputstep self._output = self._initial_output - self._outputstep
elif self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN:
self._output = self._initial_output + self._outputstep
# respect output limits # respect output limits
self._output = min(self._output, self._out_max) self._output = min(self._output, self._out_max)
@ -223,7 +223,7 @@ def main():
# Thermostat channel # Thermostat channel
channel = 0 channel = 0
# Target temperature of the autotune routine, celcius # Target temperature of the autotune routine, celcius
target_temperature = 30 target_temperature = 20
# Value by which output will be increased/decreased from zero, amps # Value by which output will be increased/decreased from zero, amps
output_step = 1 output_step = 1
# Reference period for local minima/maxima, seconds # Reference period for local minima/maxima, seconds

View File

@ -67,22 +67,16 @@ class Client:
'ki': 0.02, 'ki': 0.02,
'kd': 0.0, 'kd': 0.0,
'output_min': 0.0, 'output_min': 0.0,
'output_max': 3.0, 'output_max': 3.0},
'integral_min': -100.0, 'target': 37.0},
'integral_max': 100.0},
'target': 37.0,
'integral': 38.41138597026372},
{'channel': 1, {'channel': 1,
'parameters': { 'parameters': {
'kp': 10.0, 'kp': 10.0,
'ki': 0.02, 'ki': 0.02,
'kd': 0.0, 'kd': 0.0,
'output_min': 0.0, 'output_min': 0.0,
'output_max': 3.0, 'output_max': 3.0},
'integral_min': -100.0, 'target': 36.5}]
'integral_max': 100.0},
'target': 36.5,
'integral': nan}]
""" """
return self._get_conf("pid") return self._get_conf("pid")

View File

@ -3,7 +3,6 @@ use uom::si::{
f64::{ f64::{
ElectricPotential, ElectricPotential,
ElectricalResistance, ElectricalResistance,
ElectricCurrent,
ThermodynamicTemperature, ThermodynamicTemperature,
Time, Time,
}, },
@ -67,10 +66,10 @@ impl ChannelState {
} }
/// Update PID state on ADC input, calculate new DAC output /// Update PID state on ADC input, calculate new DAC output
pub fn update_pid(&mut self, current: ElectricCurrent) -> Option<f64> { pub fn update_pid(&mut self) -> Option<f64> {
let temperature = self.get_temperature()? let temperature = self.get_temperature()?
.get::<degree_celsius>(); .get::<degree_celsius>();
let pid_output = self.pid.update(temperature, self.get_adc_interval(), current); let pid_output = self.pid.update(temperature);
Some(pid_output) Some(pid_output)
} }

View File

@ -75,10 +75,9 @@ impl Channels {
pub fn poll_adc(&mut self, instant: Instant) -> Option<u8> { pub fn poll_adc(&mut self, instant: Instant) -> Option<u8> {
self.adc.data_ready().unwrap().map(|channel| { self.adc.data_ready().unwrap().map(|channel| {
let data = self.adc.read_data().unwrap(); let data = self.adc.read_data().unwrap();
let current = self.get_tec_i(channel.into());
let state = self.channel_state(channel); let state = self.channel_state(channel);
state.update(instant, data); state.update(instant, data);
match state.update_pid(current) { match state.update_pid() {
Some(pid_output) if state.pid_engaged => { Some(pid_output) if state.pid_engaged => {
// Forward PID output to i_set DAC // Forward PID output to i_set DAC
self.set_i(channel.into(), ElectricCurrent::new::<ampere>(pid_output)); self.set_i(channel.into(), ElectricCurrent::new::<ampere>(pid_output));
@ -437,9 +436,7 @@ impl Channels {
let tec_i = self.get_tec_i(channel); let tec_i = self.get_tec_i(channel);
let dac_value = self.get_dac(channel); let dac_value = self.get_dac(channel);
let state = self.channel_state(channel); let state = self.channel_state(channel);
let pid_output = state.pid.last_output.map(|last_output| let pid_output = ElectricCurrent::new::<ampere>(state.pid.y1);
ElectricCurrent::new::<ampere>(last_output)
);
Report { Report {
channel, channel,
time: state.get_adc_time(), time: state.get_adc_time(),
@ -541,7 +538,7 @@ pub struct Report {
i_tec: ElectricPotential, i_tec: ElectricPotential,
tec_i: ElectricCurrent, tec_i: ElectricCurrent,
tec_u_meas: ElectricPotential, tec_u_meas: ElectricPotential,
pid_output: Option<ElectricCurrent>, pid_output: ElectricCurrent,
} }
pub struct CenterPointJson(CenterPoint); pub struct CenterPointJson(CenterPoint);

View File

@ -231,10 +231,6 @@ impl Handler {
pid.parameters.output_min = value as f32, pid.parameters.output_min = value as f32,
OutputMax => OutputMax =>
pid.parameters.output_max = value as f32, 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"{}"); send_line(socket, b"{}");
Ok(Handler::Handled) Ok(Handler::Handled)

View File

@ -111,8 +111,6 @@ pub enum PidParameter {
KD, KD,
OutputMin, OutputMin,
OutputMax, OutputMax,
IntegralMin,
IntegralMax,
} }
/// Steinhart-Hart equation parameter /// Steinhart-Hart equation parameter
@ -369,8 +367,6 @@ fn pid_parameter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
value(PidParameter::KD, tag("kd")), value(PidParameter::KD, tag("kd")),
value(PidParameter::OutputMin, tag("output_min")), value(PidParameter::OutputMin, tag("output_min")),
value(PidParameter::OutputMax, tag("output_max")), value(PidParameter::OutputMax, tag("output_max")),
value(PidParameter::IntegralMin, tag("integral_min")),
value(PidParameter::IntegralMax, tag("integral_max"))
))(input)?; ))(input)?;
let (input, _) = whitespace(input)?; let (input, _) = whitespace(input)?;
let (input, value) = float(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] #[test]
fn parse_steinhart_hart() { fn parse_steinhart_hart() {
let command = Command::parse(b"s-h"); let command = Command::parse(b"s-h");

View File

@ -1,12 +1,4 @@
use serde::{Serialize, Deserialize}; use serde::{Serialize, Deserialize};
use uom::si::{
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)] #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct Parameters { pub struct Parameters {
@ -20,10 +12,6 @@ pub struct Parameters {
pub output_min: f32, pub output_min: f32,
/// Output limit maximum /// Output limit maximum
pub output_max: f32, pub output_max: f32,
/// Integral clipping minimum
pub integral_min: f32,
/// Integral clipping maximum
pub integral_max: f32
} }
impl Default for Parameters { impl Default for Parameters {
@ -34,8 +22,6 @@ impl Default for Parameters {
kd: 0.0, kd: 0.0,
output_min: -2.0, output_min: -2.0,
output_max: 2.0, output_max: 2.0,
integral_min: -10.0,
integral_max: 10.0,
} }
} }
} }
@ -43,69 +29,50 @@ impl Default for Parameters {
#[derive(Clone)] #[derive(Clone)]
pub struct Controller { pub struct Controller {
pub parameters: Parameters, pub parameters: Parameters,
pub target: f64, pub target : f64,
integral: f64, u1 : f64,
last_input: Option<f64>, x1 : f64,
pub last_output: Option<f64>, x2 : f64,
pub y1 : f64,
} }
impl Controller { impl Controller {
pub const fn new(parameters: Parameters) -> Controller { pub const fn new(parameters: Parameters) -> Controller {
Controller { Controller {
parameters: parameters, parameters: parameters,
target: 0.0, target : 0.0,
last_input: None, u1 : 0.0,
integral: 0.0, x1 : 0.0,
last_output: None, x2 : 0.0,
y1 : 0.0,
} }
} }
pub fn update(&mut self, input: f64, time_delta: Time, current: ElectricCurrent) -> f64 { // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation
let time_delta = time_delta.get::<second>(); // 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) -> f64 {
// error let mut output: f64 = self.y1 - self.target * f64::from(self.parameters.ki)
let error = self.target - input; + input * f64::from(self.parameters.kp + self.parameters.ki + self.parameters.kd)
- self.x1 * f64::from(self.parameters.kp + 2.0 * self.parameters.kd)
// proportional + self.x2 * f64::from(self.parameters.kd)
let p = f64::from(self.parameters.kp) * error; + f64::from(self.parameters.kp) * (self.target - self.u1);
// 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() &&
electric_current_error < ElectricCurrent::new::<ampere>(CURRENT_ERROR_MAX) &&
electric_current_error > -ElectricCurrent::new::<ampere>(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;
if output < self.parameters.output_min.into() { if output < self.parameters.output_min.into() {
output = self.parameters.output_min.into(); output = self.parameters.output_min.into();
} }
if output > self.parameters.output_max.into() { if output > self.parameters.output_max.into() {
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 output
} }
@ -114,17 +81,10 @@ impl Controller {
channel, channel,
parameters: self.parameters.clone(), parameters: self.parameters.clone(),
target: self.target, target: self.target,
integral: self.integral,
} }
} }
pub fn update_ki(&mut self, new_ki: f32) { 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; self.parameters.ki = new_ki;
} }
} }
@ -134,7 +94,6 @@ pub struct Summary {
channel: usize, channel: usize,
parameters: Parameters, parameters: Parameters,
target: f64, target: f64,
integral: f64,
} }
#[cfg(test)] #[cfg(test)]
@ -147,8 +106,6 @@ mod test {
kd: 0.15, kd: 0.15,
output_min: -10.0, output_min: -10.0,
output_max: 10.0, output_max: 10.0,
integral_min: -1000.0,
integral_max: 1000.0,
}; };
#[test] #[test]
@ -177,9 +134,9 @@ mod test {
while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT { while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT {
let next_t = (t + 1) % DELAY; let next_t = (t + 1) % DELAY;
// Feed the oldest temperature // Feed the oldest temperature
output = pid.update(values[next_t], Time::new::<second>(1.0), ElectricCurrent::new::<ampere>(output)); output = pid.update(values[next_t]);
// Overwrite oldest with previous temperature - 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; t = next_t;
total_t += 1; total_t += 1;
println!("{}", values[t].to_string()); println!("{}", values[t].to_string());