105 lines
2.2 KiB
Rust
105 lines
2.2 KiB
Rust
use std::time::Instant;
|
|
|
|
#[derive(Debug)]
|
|
pub struct PidController {
|
|
// Params
|
|
/// Proportional gain
|
|
kp: f64,
|
|
/// Integral gain
|
|
ki: f64,
|
|
/// Derivative gain
|
|
kd: f64,
|
|
// State
|
|
/// Proportional error
|
|
ep: f64,
|
|
/// Integral error
|
|
ei: f64,
|
|
/// Derivative error
|
|
ed: f64,
|
|
|
|
last: Option<Instant>,
|
|
}
|
|
|
|
impl PidController {
|
|
pub const fn new(kp: f64, ki: f64, kd: f64) -> Self {
|
|
Self {
|
|
kp,
|
|
ki,
|
|
kd,
|
|
ep: 0.0,
|
|
ei: 0.0,
|
|
ed: 0.0,
|
|
last: None,
|
|
}
|
|
}
|
|
|
|
pub const fn configure(&mut self, ki: f64, kp: f64, kd: f64) {
|
|
self.reset();
|
|
self.kp = kp;
|
|
self.ki = ki;
|
|
self.kd = kd;
|
|
}
|
|
|
|
pub const fn get_kp(&self) -> f64 {
|
|
self.kp
|
|
}
|
|
|
|
pub const fn get_ki(&self) -> f64 {
|
|
self.ki
|
|
}
|
|
|
|
pub const fn get_kd(&self) -> f64 {
|
|
self.kd
|
|
}
|
|
|
|
pub fn update(&mut self, error: f64, t: Instant) -> f64 {
|
|
match self.last {
|
|
// First update
|
|
None => {
|
|
// Calculate error
|
|
let ep = error;
|
|
|
|
// Calculate signal
|
|
let signal = self.kp * ep;
|
|
|
|
// Set values
|
|
self.ep = ep;
|
|
self.ei = 0.0;
|
|
|
|
self.ed = 0.0;
|
|
self.last = Some(t);
|
|
|
|
signal
|
|
}
|
|
// Subsequent updates
|
|
Some(last) => {
|
|
// Calculate the time delta in seconds
|
|
let dt = t.duration_since(last).as_secs_f64();
|
|
|
|
// Calculate errors
|
|
let ep = error;
|
|
let ei = ep.mul_add(dt, self.ei);
|
|
let ed = (ep - self.ep) / dt;
|
|
|
|
// Calculate signal
|
|
let signal = self.kd.mul_add(ed, self.kp.mul_add(ep, self.ki * ei));
|
|
|
|
// Set values
|
|
self.ep = ep;
|
|
self.ei = ei;
|
|
self.ed = ed;
|
|
self.last = Some(t);
|
|
|
|
signal
|
|
}
|
|
}
|
|
}
|
|
|
|
pub const fn reset(&mut self) {
|
|
self.ep = 0.0;
|
|
self.ei = 0.0;
|
|
self.ed = 0.0;
|
|
self.last = None;
|
|
}
|
|
}
|