ionpak-thermostat/firmware/src/pid.rs

75 lines
1.8 KiB
Rust
Raw Normal View History

2017-05-09 15:57:54 +08:00
use core::num::Float;
2017-05-06 17:17:41 +08:00
#[derive(Clone, Copy)]
pub struct Parameters {
pub kp: f32,
pub ki: f32,
pub kd: f32,
pub output_min: f32,
pub output_max: f32,
pub integral_min: f32,
pub integral_max: f32
}
pub struct Controller {
parameters: Parameters,
pub target: f32,
2017-05-06 17:17:41 +08:00
integral: f32,
pub last_input: Option<f32>
2017-05-06 17:17:41 +08:00
}
impl Controller {
pub const fn new(parameters: Parameters) -> Controller {
Controller {
parameters: parameters,
target: 0.0,
2017-05-06 20:43:33 +08:00
last_input: None,
2017-05-06 17:17:41 +08:00
integral: 0.0
}
}
pub fn update(&mut self, input: f32) -> f32 {
let error = self.target - input;
let p = self.parameters.kp * error;
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;
2017-05-06 20:43:33 +08:00
let d = match self.last_input {
None => 0.0,
Some(last_input) => self.parameters.kd * (last_input - input)
2017-05-06 17:17:41 +08:00
};
2017-05-06 20:43:33 +08:00
self.last_input = Some(input);
2017-05-06 17:17:41 +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;
}
output
}
2017-05-09 15:57:54 +08:00
#[allow(dead_code)]
2017-05-09 19:06:42 +08:00
pub fn is_within(&self, tolerance: f32) -> bool {
2017-05-09 15:57:54 +08:00
match self.last_input {
None => false,
Some(last_input) => (last_input - self.target).abs() < tolerance
}
}
2017-05-06 17:17:41 +08:00
#[allow(dead_code)]
pub fn reset(&mut self) {
self.integral = 0.0;
2017-05-06 20:43:33 +08:00
self.last_input = None;
2017-05-06 17:17:41 +08:00
}
}