73 lines
1.7 KiB
Rust
73 lines
1.7 KiB
Rust
|
use core::f32;
|
||
|
|
use core::num::Float;
|
||
|
|
|
||
|
|
#[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,
|
||
|
|
target: f32,
|
||
|
|
integral: f32,
|
||
|
|
last_input: f32
|
||
|
|
}
|
||
|
|
|
||
|
|
impl Controller {
|
||
|
|
pub const fn new(parameters: Parameters) -> Controller {
|
||
|
|
Controller {
|
||
|
|
parameters: parameters,
|
||
|
|
target: 0.0,
|
||
|
|
last_input: f32::NAN,
|
||
|
|
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;
|
||
|
|
|
||
|
|
let d = if self.last_input.is_nan() {
|
||
|
|
0.0
|
||
|
|
} else {
|
||
|
|
self.parameters.kd * (self.last_input - input)
|
||
|
|
};
|
||
|
|
self.last_input = input;
|
||
|
|
|
||
|
|
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
|
||
|
|
}
|
||
|
|
|
||
|
|
pub fn set_target(&mut self, target: f32) {
|
||
|
|
self.target = target
|
||
|
|
}
|
||
|
|
|
||
|
|
#[allow(dead_code)]
|
||
|
|
pub fn reset(&mut self) {
|
||
|
|
self.integral = 0.0;
|
||
|
|
self.last_input = f32::NAN;
|
||
|
|
}
|
||
|
|
}
|