thermostat/src/pid.rs

190 lines
5.6 KiB
Rust
Raw Normal View History

2020-09-24 07:18:33 +08:00
use serde::{Serialize, Deserialize};
use uom::si::{
f64::{Time, ElectricCurrent},
time::second,
electric_current::ampere,
};
2020-09-24 07:18:33 +08:00
/// Allowable current error for integral accumulation
const CURRENT_ERROR_MAX: f64 = 0.1;
2020-09-25 03:35:15 +08:00
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
2020-03-19 04:51:30 +08:00
pub struct Parameters {
2020-12-18 23:29:53 +08:00
/// Gain coefficient for proportional term
pub kp: f32,
2020-12-18 23:29:53 +08:00
/// Gain coefficient for integral term
pub ki: f32,
2020-12-18 23:29:53 +08:00
/// Gain coefficient for derivative term
pub kd: f32,
2020-12-18 23:29:53 +08:00
/// Output limit minimum
pub output_min: f32,
2020-12-18 23:29:53 +08:00
/// Output limit maximum
pub output_max: f32,
2020-12-18 23:29:53 +08:00
/// Integral clipping minimum
pub integral_min: f32,
2020-12-18 23:29:53 +08:00
/// Integral clipping maximum
pub integral_max: f32
2020-03-19 04:51:30 +08:00
}
2020-03-20 01:34:57 +08:00
impl Default for Parameters {
fn default() -> Self {
Parameters {
2021-01-08 11:31:33 +08:00
kp: 0.0,
ki: 0.0,
kd: 0.0,
output_min: -2.0,
2020-09-18 06:41:32 +08:00
output_max: 2.0,
integral_min: -10.0,
integral_max: 10.0,
2020-03-20 01:34:57 +08:00
}
}
}
2020-03-19 04:51:30 +08:00
#[derive(Clone)]
pub struct Controller {
2020-03-20 05:00:22 +08:00
pub parameters: Parameters,
pub target: f64,
integral: f64,
2020-03-20 05:00:22 +08:00
last_input: Option<f64>,
pub last_output: Option<f64>,
2020-03-19 04:51:30 +08:00
}
impl Controller {
pub const fn new(parameters: Parameters) -> Controller {
Controller {
parameters: parameters,
target: 0.0,
last_input: None,
2020-03-20 05:00:22 +08:00
integral: 0.0,
last_output: None,
2020-03-19 04:51:30 +08:00
}
}
pub fn update(&mut self, input: f64, time_delta: Time, current: ElectricCurrent) -> f64 {
let time_delta = time_delta.get::<second>();
2020-09-18 06:09:30 +08:00
// error
let error = self.target - input;
2020-03-19 04:51:30 +08:00
2020-12-26 11:01:40 +08:00
// proportional
let p = f64::from(self.parameters.kp) * error;
2020-03-19 04:51:30 +08:00
// 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();
2020-03-19 04:51:30 +08:00
}
if self.integral > self.parameters.integral_max.into() {
self.integral = self.parameters.integral_max.into();
2020-03-19 04:51:30 +08:00
}
let i = self.integral * f64::from(self.parameters.ki);
2020-03-19 04:51:30 +08:00
2020-09-18 06:09:30 +08:00
// derivative
2020-03-19 04:51:30 +08:00
let d = match self.last_input {
None =>
0.0,
Some(last_input) =>
f64::from(self.parameters.kd) * (last_input - input) / time_delta,
2020-03-19 04:51:30 +08:00
};
self.last_input = Some(input);
2020-09-18 06:09:30 +08:00
// output
2020-03-19 04:51:30 +08:00
let mut output = p + i + d;
if output < self.parameters.output_min.into() {
output = self.parameters.output_min.into();
2020-03-19 04:51:30 +08:00
}
if output > self.parameters.output_max.into() {
output = self.parameters.output_max.into();
2020-03-19 04:51:30 +08:00
}
2020-03-20 05:00:22 +08:00
self.last_output = Some(output);
2020-03-19 04:51:30 +08:00
output
}
2020-10-01 02:06:47 +08:00
pub fn summary(&self, channel: usize) -> Summary {
Summary {
channel,
parameters: self.parameters.clone(),
target: self.target,
integral: self.integral,
}
}
2021-01-08 11:25:01 +08:00
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;
}
2020-10-01 02:06:47 +08:00
}
#[derive(Clone, Serialize, Deserialize)]
pub struct Summary {
channel: usize,
parameters: Parameters,
target: f64,
integral: f64,
}
2020-03-19 04:51:30 +08:00
#[cfg(test)]
mod test {
use super::*;
const PARAMETERS: Parameters = Parameters {
kp: 0.03,
ki: 0.002,
kd: 0.15,
2020-03-19 04:51:30 +08:00
output_min: -10.0,
output_max: 10.0,
integral_min: -1000.0,
integral_max: 1000.0,
2020-03-19 04:51:30 +08:00
};
#[test]
fn test_controller() {
// Initial and ambient temperature
const DEFAULT: f64 = 20.0;
// Target temperature
const TARGET: f64 = 40.0;
// Control tolerance
const ERROR: f64 = 0.01;
// System response delay
const DELAY: usize = 10;
// Heat lost
const LOSS: f64 = 0.05;
// Limit simulation cycle, reaching this limit before settling fails test
const CYCLE_LIMIT: u32 = 1000;
2020-03-19 04:51:30 +08:00
let mut pid = Controller::new(PARAMETERS.clone());
pid.target = TARGET;
2020-03-19 04:51:30 +08:00
let mut values = [DEFAULT; DELAY];
let mut t = 0;
let mut total_t = 0;
2021-01-16 11:04:24 +08:00
let mut output: f64 = 0.0;
2020-03-19 04:51:30 +08:00
let target = (TARGET - ERROR)..=(TARGET + ERROR);
while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT {
2020-03-19 04:51:30 +08:00
let next_t = (t + 1) % DELAY;
// Feed the oldest temperature
2021-01-16 11:04:24 +08:00
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;
2020-03-19 04:51:30 +08:00
t = next_t;
total_t += 1;
println!("{}", values[t].to_string());
2020-03-19 04:51:30 +08:00
}
assert_ne!(CYCLE_LIMIT, total_t);
2020-03-19 04:51:30 +08:00
}
}