thermostat/src/pid.rs

128 lines
3.1 KiB
Rust
Raw Normal View History

2020-03-19 04:51:30 +08:00
#[derive(Clone, Copy)]
pub struct Parameters {
pub kp: f64,
pub ki: f64,
pub kd: f64,
pub output_min: f64,
pub output_max: f64,
pub integral_min: f64,
pub integral_max: f64
2020-03-19 04:51:30 +08:00
}
2020-03-20 01:34:57 +08:00
impl Default for Parameters {
fn default() -> Self {
Parameters {
2020-09-18 06:41:32 +08:00
kp: 1.5,
ki: 0.1,
kd: 150.0,
2020-03-20 01:34:57 +08:00
output_min: 0.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) -> f64 {
2020-09-18 06:09:30 +08:00
// error
2020-03-19 04:51:30 +08:00
let error = self.target - input;
2020-09-18 06:09:30 +08:00
// partial
2020-03-19 04:51:30 +08:00
let p = self.parameters.kp * error;
2020-09-18 06:09:30 +08:00
//integral
2020-03-19 04:51:30 +08:00
self.integral += error;
if self.integral < self.parameters.integral_min {
self.integral = self.parameters.integral_min;
}
if self.integral > self.parameters.integral_max {
self.integral = self.parameters.integral_max;
}
let i = self.parameters.ki * self.integral;
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) => self.parameters.kd * (last_input - input)
};
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 {
output = self.parameters.output_min;
}
if output > self.parameters.output_max {
output = self.parameters.output_max;
}
2020-03-20 05:00:22 +08:00
self.last_output = Some(output);
2020-03-19 04:51:30 +08:00
output
}
#[allow(dead_code)]
pub fn reset(&mut self) {
self.integral = 0.0;
self.last_input = None;
}
}
#[cfg(test)]
mod test {
use super::*;
const PARAMETERS: Parameters = Parameters {
kp: 0.055,
ki: 0.005,
kd: 0.04,
output_min: -10.0,
output_max: 10.0,
integral_min: -100.0,
integral_max: 100.0,
};
#[test]
fn test_controller() {
const DEFAULT: f64 = 0.0;
const TARGET: f64 = 1234.56;
const ERROR: f64 = 0.01;
2020-03-19 04:51:30 +08:00
const DELAY: usize = 10;
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;
let target = (TARGET - ERROR)..=(TARGET + ERROR);
while !values.iter().all(|value| target.contains(value)) {
let next_t = (t + 1) % DELAY;
// Feed the oldest temperature
let output = pid.update(values[next_t]);
// Overwrite oldest with previous temperature + output
values[next_t] = values[t] + output;
t = next_t;
total_t += 1;
}
}
}