M-Labs/thermostat
9
0
Fork 5
Code Issues 9 Pull Requests 5 Releases Wiki Activity

BREAKING, NOT TESTED on hardware, rewrite PID

Browse Source
This commit is contained in:
topquark12 2022-02-08 22:39:55 +08:00
parent 69dabf5aa1
commit b2a58a6703
4 changed files with 31 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/channels.rs
View File

@ -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::<ampere>(last_output)
);
let pid_output = ElectricCurrent::new::<ampere>(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<ElectricCurrent>,
pid_output: ElectricCurrent,
}
pub struct CenterPointJson(CenterPoint);

4
src/command_handler.rs
View File

@ -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)

14
src/command_parser.rs
View File

@ -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<Command, Error>> {
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");

95
src/pid.rs
View File

@ -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<f64>,
pub last_output: Option<f64>,
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::<second>();
// 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::<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;
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::<second>(1.0), ElectricCurrent::new::<ampere>(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());