#![no_std]
extern crate num_traits;
use num_traits::float::FloatCore;
#[derive(Debug)]
pub struct Pid<T: FloatCore> {
pub kp: T,
pub ki: T,
pub kd: T,
pub p_limit: T,
pub i_limit: T,
pub d_limit: T,
setpoint: Option<T>,
prev_measurement: Option<T>,
integral_term: T,
}
#[derive(Debug)]
pub struct ControlOutput<T: FloatCore> {
pub p: T,
pub i: T,
pub d: T,
pub output: T,
}
impl<T> Pid<T>
where
T: FloatCore,
{
pub fn new(kp: T, ki: T, kd: T, p_limit: T, i_limit: T, d_limit: T) -> Self {
Self {
kp,
ki,
kd,
p_limit,
i_limit,
d_limit,
setpoint: None,
prev_measurement: None,
integral_term: T::zero(),
}
}
pub fn update_setpoint(&mut self, setpoint: T) {
self.setpoint = Some(setpoint);
}
pub fn get_setpoint(&mut self) -> Option<T> {
self.setpoint
}
pub fn reset_integral_term(&mut self) {
self.integral_term = T::zero();
}
pub fn next_control_output(&mut self, measurement: T) -> ControlOutput<T> {
if self.setpoint.is_none() {
panic!("No set point specified.");
}
let error = self.setpoint.unwrap() - measurement;
let p_unbounded = error * self.kp;
let p = self.p_limit.min(p_unbounded.abs()) * p_unbounded.signum();
self.integral_term = self.integral_term + error * self.ki;
self.integral_term =
self.i_limit.min(self.integral_term.abs()) * self.integral_term.signum();
let d_unbounded = -match self.prev_measurement.as_ref() {
Some(prev_measurement) => measurement - *prev_measurement,
None => T::zero(),
} * self.kd;
self.prev_measurement = Some(measurement);
let d = self.d_limit.min(d_unbounded.abs()) * d_unbounded.signum();
ControlOutput {
p,
i: self.integral_term,
d,
output: (p + self.integral_term + d),
}
}
}
#[cfg(test)]
mod tests {
use super::Pid;
#[test]
fn proportional() {
let mut pid = Pid::new(2.0, 0.0, 0.0, 100.0, 100.0, 100.0);
assert_eq!(pid.get_setpoint(), None);
pid.update_setpoint(10.0);
assert_eq!(pid.get_setpoint(), Some(10.0));
assert_eq!(pid.next_control_output(0.0).output, 20.0);
pid.p_limit = 10.0;
assert_eq!(pid.next_control_output(0.0).output, 10.0);
}
#[test]
fn derivative() {
let mut pid = Pid::new(0.0, 0.0, 2.0, 100.0, 100.0, 100.0);
pid.update_setpoint(10.0);
assert_eq!(pid.next_control_output(0.0).output, 0.0);
assert_eq!(pid.next_control_output(5.0).output, -10.0);
pid.d_limit = 5.0;
assert_eq!(pid.next_control_output(10.0).output, -5.0);
}
#[test]
fn integral() {
let mut pid = Pid::new(0.0, 2.0, 0.0, 100.0, 100.0, 100.0);
pid.update_setpoint(10.0);
assert_eq!(pid.next_control_output(0.0).output, 20.0);
assert_eq!(pid.next_control_output(0.0).output, 40.0);
assert_eq!(pid.next_control_output(5.0).output, 50.0);
pid.i_limit = 50.0;
assert_eq!(pid.next_control_output(5.0).output, 50.0);
assert_eq!(pid.next_control_output(15.0).output, 40.0);
let mut pid2 = Pid::new(0.0, 2.0, 0.0, 100.0, 100.0, 100.0);
pid2.update_setpoint(-10.0);
assert_eq!(pid2.next_control_output(0.0).output, -20.0);
assert_eq!(pid2.next_control_output(0.0).output, -40.0);
pid2.i_limit = 50.0;
assert_eq!(pid2.next_control_output(-5.0).output, -50.0);
assert_eq!(pid2.next_control_output(-15.0).output, -40.0);
}
#[test]
fn pid() {
let mut pid = Pid::new(1.0, 0.1, 1.0, 100.0, 100.0, 100.0);
pid.update_setpoint(10.0);
let out = pid.next_control_output(0.0);
assert_eq!(out.p, 10.0);
assert_eq!(out.i, 1.0);
assert_eq!(out.d, 0.0);
assert_eq!(out.output, 11.0);
let out = pid.next_control_output(5.0);
assert_eq!(out.p, 5.0);
assert_eq!(out.i, 1.5);
assert_eq!(out.d, -5.0);
assert_eq!(out.output, 1.5);
let out = pid.next_control_output(11.0);
assert_eq!(out.p, -1.0);
assert_eq!(out.i, 1.4);
assert_eq!(out.d, -6.0);
assert_eq!(out.output, -5.6);
let out = pid.next_control_output(10.0);
assert_eq!(out.p, 0.0);
assert_eq!(out.i, 1.4);
assert_eq!(out.d, 1.0);
assert_eq!(out.output, 2.4);
}
#[test]
fn f32_and_f64() {
let mut pid32 = Pid::new(2.0f32, 0.0, 0.0, 100.0, 100.0, 100.0);
pid32.update_setpoint(10.0);
let mut pid64 = Pid::new(2.0f64, 0.0, 0.0, 100.0, 100.0, 100.0);
pid64.update_setpoint(10.0);
assert_eq!(
pid32.next_control_output(0.0).output,
pid64.next_control_output(0.0).output as f32
);
assert_eq!(
pid32.next_control_output(0.0).output as f64,
pid64.next_control_output(0.0).output
);
}
}