Rusty PID

pid.rs

use core::ops::*;

pub struct Regulator<F>
where
    F: Default + Add<Output = F> + Sub<Output = F> + Mul<Output = F> + PartialOrd + Copy,
{
    kp: F,
    ki: F,
    kd: F,
    last_error: F,
    error_sum: F,
    min_error_sum: F,
    max_error_sum: F,
}

impl<F> Regulator<F>
where
    F: Default + Add<Output = F> + Sub<Output = F> + Mul<Output = F> + PartialOrd + Copy,
{
    pub fn new(
        kp: impl Into<F>,
        ki: impl Into<F>,
        kd: impl Into<F>,
        min_error_sum: impl Into<F>,
        max_error_sum: impl Into<F>,
    ) -> Self {
        Self {
            kp: kp.into(),
            ki: ki.into(),
            kd: kd.into(),
            min_error_sum: min_error_sum.into(),
            max_error_sum: max_error_sum.into(),
            last_error: F::default(),
            error_sum: F::default(),
        }
    }

    pub fn update(&mut self, sp: impl Into<F>, val: impl Into<F>) -> F {
        let error = sp.into() - val.into();
        let error_delta = error - self.last_error;

        self.last_error = error;
        self.error_sum = self.error_sum + error;
        if self.error_sum < self.min_error_sum {
            self.error_sum = self.min_error_sum
        }
        if self.error_sum > self.max_error_sum {
            self.error_sum = self.max_error_sum
        }

        let p = error * self.kp;
        let i = self.error_sum * self.ki;
        let d = error_delta * self.kd;

        p + i + d
    }
}