2020-09-24 07:18:33 +08:00
|
|
|
use serde::{Serialize, Deserialize};
|
2020-12-18 22:40:05 +08:00
|
|
|
use uom::si::{
|
|
|
|
f64::Time,
|
|
|
|
time::second,
|
|
|
|
};
|
2020-09-24 07:18:33 +08:00
|
|
|
|
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
|
2020-09-25 02:45:07 +08:00
|
|
|
pub kp: f32,
|
2020-12-18 23:29:53 +08:00
|
|
|
/// Gain coefficient for integral term
|
2020-09-25 02:45:07 +08:00
|
|
|
pub ki: f32,
|
2020-12-18 23:29:53 +08:00
|
|
|
/// Gain coefficient for derivative term
|
2020-09-25 02:45:07 +08:00
|
|
|
pub kd: f32,
|
2020-12-18 23:29:53 +08:00
|
|
|
/// Output limit minimum
|
2020-09-25 02:45:07 +08:00
|
|
|
pub output_min: f32,
|
2020-12-18 23:29:53 +08:00
|
|
|
/// Output limit maximum
|
2020-09-25 02:45:07 +08:00
|
|
|
pub output_max: f32,
|
2020-12-18 23:29:53 +08:00
|
|
|
/// Integral clipping minimum
|
2020-09-25 02:45:07 +08:00
|
|
|
pub integral_min: f32,
|
2020-12-18 23:29:53 +08:00
|
|
|
/// Integral clipping maximum
|
2020-09-25 02:45:07 +08:00
|
|
|
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 {
|
2020-09-18 06:41:32 +08:00
|
|
|
kp: 1.5,
|
2020-12-18 22:45:16 +08:00
|
|
|
ki: 1.0,
|
|
|
|
kd: 1.5,
|
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,
|
2020-03-19 04:56:52 +08:00
|
|
|
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
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-12-18 22:40:05 +08:00
|
|
|
pub fn update(&mut self, input: f64, time_delta: Time) -> f64 {
|
|
|
|
let time_delta = time_delta.get::<second>();
|
|
|
|
|
2020-09-18 06:09:30 +08:00
|
|
|
// error
|
2020-12-26 11:47:21 +08:00
|
|
|
let error = self.target - input;
|
2020-03-19 04:51:30 +08:00
|
|
|
|
2020-12-26 11:01:40 +08:00
|
|
|
// proportional
|
2020-09-25 02:45:07 +08:00
|
|
|
let p = f64::from(self.parameters.kp) * error;
|
2020-03-19 04:51:30 +08:00
|
|
|
|
2020-10-11 07:56:22 +08:00
|
|
|
// integral
|
2020-12-26 11:47:21 +08:00
|
|
|
if let Some(last_output_val) = self.last_output {
|
|
|
|
// anti integral windup
|
|
|
|
if last_output_val < self.parameters.output_max.into() && last_output_val > self.parameters.output_min.into() {
|
|
|
|
self.integral += error * time_delta;
|
|
|
|
}
|
|
|
|
}
|
2020-09-25 02:45:07 +08:00
|
|
|
if self.integral < self.parameters.integral_min.into() {
|
|
|
|
self.integral = self.parameters.integral_min.into();
|
2020-03-19 04:51:30 +08:00
|
|
|
}
|
2020-09-25 02:45:07 +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
|
|
|
}
|
2020-12-26 11:47:21 +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 {
|
2020-12-18 22:40:05 +08:00
|
|
|
None =>
|
|
|
|
0.0,
|
|
|
|
Some(last_input) =>
|
2020-12-29 17:09:03 +08:00
|
|
|
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;
|
2020-09-25 02:45:07 +08:00
|
|
|
if output < self.parameters.output_min.into() {
|
|
|
|
output = self.parameters.output_min.into();
|
2020-03-19 04:51:30 +08:00
|
|
|
}
|
2020-09-25 02:45:07 +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,
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#[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 {
|
2020-12-28 17:38:10 +08:00
|
|
|
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,
|
2020-12-28 17:38:10 +08:00
|
|
|
integral_min: -1000.0,
|
|
|
|
integral_max: 1000.0,
|
2020-03-19 04:51:30 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
#[test]
|
|
|
|
fn test_controller() {
|
2020-12-28 17:38:10 +08:00
|
|
|
// 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());
|
2020-09-17 07:48:27 +08:00
|
|
|
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);
|
2020-12-28 17:38:10 +08:00
|
|
|
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
|
2020-12-21 03:24:24 +08:00
|
|
|
let output = pid.update(values[next_t], Time::new::<second>(1.0));
|
2020-10-12 05:12:18 +08:00
|
|
|
// Overwrite oldest with previous temperature - output
|
2020-12-28 17:38:10 +08:00
|
|
|
values[next_t] = values[t] + output - (values[t] - DEFAULT) * LOSS;
|
2020-03-19 04:51:30 +08:00
|
|
|
t = next_t;
|
|
|
|
total_t += 1;
|
2020-12-28 17:38:10 +08:00
|
|
|
println!("{}", values[t].to_string());
|
2020-03-19 04:51:30 +08:00
|
|
|
}
|
2020-12-28 17:38:10 +08:00
|
|
|
assert_ne!(CYCLE_LIMIT, total_t);
|
2020-03-19 04:51:30 +08:00
|
|
|
}
|
|
|
|
}
|