pid_controller/
lib.rs

1use std::ops::{Add, Div, Mul, Sub};
2
3use num_traits::One;
4
5#[derive(Clone, Copy, Debug)]
6
7/// Proportional controller
8pub struct P<T> {
9    pub kp: T,
10}
11
12impl<T: One> Default for P<T> {
13    fn default() -> Self {
14        Self { kp: T::one() }
15    }
16}
17
18impl<T> P<T> {
19    // Between 0.7 and 1
20    pub fn new(damping_ratio: T, time_constant: T) -> Self
21    where
22        T: One + Add<Output = T> + Div<Output = T> + Copy,
23    {
24        let kp = (T::one() / (time_constant * time_constant))
25            * (T::one() + (T::one() + T::one()) * damping_ratio);
26        Self { kp }
27    }
28
29    pub fn control<U>(self, target: U, actual: U) -> T::Output
30    where
31        T: Mul<U> + Clone,
32        U: Sub<Output = U>,
33    {
34        let error = error(target, actual);
35        self.control_with_error(error)
36    }
37
38    pub fn control_with_error<U>(&self, error: U) -> T::Output
39    where
40        T: Mul<U> + Clone,
41    {
42        self.kp.clone() * error
43    }
44}
45
46#[derive(Clone, Debug)]
47pub struct PD<T> {
48    pub p: P<T>,
49    pub kd: T,
50}
51
52impl<T> PD<T> {
53    pub fn new(damping_ratio: T, time_constant: T) -> Self
54    where
55        T: One + Add<Output = T> + Div<Output = T> + Copy,
56    {
57        let kd = (T::one() / time_constant)
58            * (T::one() + (T::one() + T::one()) * time_constant * time_constant);
59        Self {
60            p: P::new(damping_ratio, time_constant),
61            kd,
62        }
63    }
64
65    pub fn control<U>(&self, target: U, target_dot: U, actual: U, actual_dot: U) -> U
66    where
67        T: Mul<U, Output = U> + Sub<Output = T> + Clone,
68        U: Add<Output = U> + Sub<Output = U>,
69    {
70        let p = self.p.clone().control(target, actual);
71        self.control_with_p(p, target_dot, actual_dot)
72    }
73
74    pub fn control_with_p<U>(&self, p: U, target_dot: U, actual_dot: U) -> U
75    where
76        T: Mul<U, Output = U> + Clone,
77        U: Add<Output = U> + Sub<Output = U>,
78    {
79        self.kd.clone() * (target_dot - actual_dot) + p
80    }
81}
82
83pub fn error<T>(target: T, actual: T) -> T
84where
85    T: Sub<Output = T>,
86{
87    target - actual
88}
89
90#[derive(Clone, Debug)]
91pub struct PID {
92    pub pd: PD<f32>,
93    pub ki: f32,
94    pub integrated_error: f32,
95}
96
97impl PID {
98    pub fn new(damping_ratio: f32, time_constant: f32) -> Self {
99        let ki = 1. / time_constant.powi(3);
100        Self {
101            pd: PD::new(damping_ratio, time_constant),
102            ki,
103            integrated_error: 0.,
104        }
105    }
106
107    pub fn control(
108        &mut self,
109        target: f32,
110        target_dot: f32,
111        actual: f32,
112        actual_dot: f32,
113        dt: f32,
114    ) -> f32 {
115        let error = error(target, actual);
116        self.integrated_error += error * dt;
117
118        let p = self.pd.p.control_with_error(error);
119        let i = self.integrated_error * self.ki;
120        let d = self.pd.control_with_p(p, target_dot, actual_dot);
121
122        p + i + d
123    }
124}
125
126#[cfg(test)]
127mod tests {
128    #[test]
129    fn it_works() {
130        let result = 2 + 2;
131        assert_eq!(result, 4);
132    }
133}
pid_controller/lib.rs

pid_controller/
lib.rs
