cu_pid/
lib.rs

1use bincode::de::Decoder;
2use bincode::enc::Encoder;
3use bincode::error::{DecodeError, EncodeError};
4use bincode::{Decode, Encode};
5use cu29::prelude::*;
6use std::marker::PhantomData;
7
8/// Output of the PID controller.
9#[derive(Debug, Default, Clone, Encode, Decode)]
10pub struct PIDControlOutputPayload {
11    /// Proportional term
12    pub p: f32,
13    /// Integral term
14    pub i: f32,
15    /// Derivative term
16    pub d: f32,
17    /// Final output
18    pub output: f32,
19}
20
21/// This is the underlying standard PID controller.
22pub struct PIDController {
23    // Configuration
24    kp: f32,
25    ki: f32,
26    kd: f32,
27    setpoint: f32,
28    p_limit: f32,
29    i_limit: f32,
30    d_limit: f32,
31    output_limit: f32,
32    sampling: CuDuration,
33    // Internal state
34    integral: f32,
35    last_error: f32,
36    elapsed: CuDuration,
37    last_output: PIDControlOutputPayload,
38}
39
40impl PIDController {
41    #[allow(clippy::too_many_arguments)]
42    pub fn new(
43        kp: f32,
44        ki: f32,
45        kd: f32,
46        setpoint: f32,
47        p_limit: f32,
48        i_limit: f32,
49        d_limit: f32,
50        output_limit: f32,
51        sampling: CuDuration, // to avoid oversampling and get a bunch of zeros.
52    ) -> Self {
53        PIDController {
54            kp,
55            ki,
56            kd,
57            setpoint,
58            integral: 0.0,
59            last_error: 0.0,
60            p_limit,
61            i_limit,
62            d_limit,
63            output_limit,
64            elapsed: CuDuration::default(),
65            sampling,
66            last_output: PIDControlOutputPayload::default(),
67        }
68    }
69
70    pub fn reset(&mut self) {
71        self.integral = 0.0f32;
72        self.last_error = 0.0f32;
73    }
74
75    pub fn init_measurement(&mut self, measurement: f32) {
76        self.last_error = self.setpoint - measurement;
77        self.elapsed = self.sampling; // force the computation on the first next_control_output
78    }
79
80    pub fn next_control_output(
81        &mut self,
82        measurement: f32,
83        dt: CuDuration,
84    ) -> PIDControlOutputPayload {
85        self.elapsed += dt;
86
87        if self.elapsed < self.sampling {
88            // if we bang too fast the PID controller, just keep on giving the same answer
89            return self.last_output.clone();
90        }
91
92        let error = self.setpoint - measurement;
93        let CuDuration(elapsed) = self.elapsed;
94        let dt = elapsed as f32 / 1_000_000f32; // the unit is kind of arbitrary.
95
96        // Proportional term
97        let p_unbounded = self.kp * error;
98        let p = p_unbounded.clamp(-self.p_limit, self.p_limit);
99
100        // Integral term (accumulated over time)
101        self.integral += error * dt;
102        let i_unbounded = self.ki * self.integral;
103        let i = i_unbounded.clamp(-self.i_limit, self.i_limit);
104
105        // Derivative term (rate of change)
106        let derivative = (error - self.last_error) / dt;
107        let d_unbounded = self.kd * derivative;
108        let d = d_unbounded.clamp(-self.d_limit, self.d_limit);
109
110        // Update last error for next calculation
111        self.last_error = error;
112
113        // Final output: sum of P, I, D with output limit
114        let output_unbounded = p + i + d;
115        let output = output_unbounded.clamp(-self.output_limit, self.output_limit);
116
117        let output = PIDControlOutputPayload { p, i, d, output };
118
119        self.last_output = output.clone();
120        self.elapsed = CuDuration::default();
121        output
122    }
123}
124
125/// This is the Copper task encapsulating the PID controller.
126pub struct GenericPIDTask<I>
127where
128    f32: for<'a> From<&'a I>,
129{
130    _marker: PhantomData<I>,
131    pid: PIDController,
132    first_run: bool,
133    last_tov: CuTime,
134    setpoint: f32,
135    cutoff: f32,
136}
137
138impl<'cl, I> CuTask<'cl> for GenericPIDTask<I>
139where
140    f32: for<'a> From<&'a I>,
141    I: CuMsgPayload + 'cl,
142{
143    type Input = input_msg!('cl, I);
144    type Output = output_msg!('cl, PIDControlOutputPayload);
145
146    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
147    where
148        Self: Sized,
149    {
150        match config {
151            Some(config) => {
152                debug!("PIDTask config: {:?}", config);
153                let setpoint: f32 = config
154                    .get::<f64>("setpoint")
155                    .ok_or("'setpoint' not found in config")?
156                    as f32;
157
158                let cutoff: f32 = config.get::<f64>("cutoff").ok_or(
159                    "'cutoff' not found in config, please set an operating +/- limit on the input.",
160                )? as f32;
161
162                // p is mandatory
163                let kp = if let Some(kp) = config.get::<f64>("kp") {
164                    Ok(kp as f32)
165                } else {
166                    Err(CuError::from(
167                        "'kp' not found in the config. We need at least 'kp' to make the PID algorithm work.",
168                    ))
169                }?;
170
171                let p_limit = getcfg(config, "pl", 2.0f32);
172                let ki = getcfg(config, "ki", 0.0f32);
173                let i_limit = getcfg(config, "il", 1.0f32);
174                let kd = getcfg(config, "kd", 0.0f32);
175                let d_limit = getcfg(config, "dl", 2.0f32);
176                let output_limit = getcfg(config, "ol", 1.0f32);
177
178                let sampling = if let Some(value) = config.get::<u32>("sampling_ms") {
179                    CuDuration::from(value as u64 * 1_000_000u64)
180                } else {
181                    CuDuration::default()
182                };
183
184                let pid: PIDController = PIDController::new(
185                    kp,
186                    ki,
187                    kd,
188                    setpoint,
189                    p_limit,
190                    i_limit,
191                    d_limit,
192                    output_limit,
193                    sampling,
194                );
195
196                Ok(Self {
197                    _marker: PhantomData,
198                    pid,
199                    first_run: true,
200                    last_tov: CuTime::default(),
201                    setpoint,
202                    cutoff,
203                })
204            }
205            None => Err(CuError::from("PIDTask needs a config.")),
206        }
207    }
208
209    fn process(
210        &mut self,
211        _clock: &RobotClock,
212        input: Self::Input,
213        output: Self::Output,
214    ) -> CuResult<()> {
215        match input.payload() {
216            Some(payload) => {
217                let tov = match input.metadata.tov {
218                    Tov::Time(single) => single,
219                    _ => return Err("Unexpected variant for a TOV of PID".into()),
220                };
221
222                let measure: f32 = payload.into();
223
224                if self.first_run {
225                    self.first_run = false;
226                    self.last_tov = tov;
227                    self.pid.init_measurement(measure);
228                    output.clear_payload();
229                    return Ok(());
230                }
231                let dt = tov - self.last_tov;
232                self.last_tov = tov;
233
234                // update the status of the pid.
235                let state = self.pid.next_control_output(measure, dt);
236                // But safety check if the input is within operational margins and cut power if it is not.
237                if measure > self.setpoint + self.cutoff {
238                    return Err(
239                        format!("{} > {} (cutoff)", measure, self.setpoint + self.cutoff).into(),
240                    );
241                }
242                if measure < self.setpoint - self.cutoff {
243                    return Err(
244                        format!("{} < {} (cutoff)", measure, self.setpoint - self.cutoff).into(),
245                    );
246                }
247                output.metadata.set_status(format!(
248                    "{:>5.2} {:>5.2} {:>5.2} {:>5.2}",
249                    &state.output, &state.p, &state.i, &state.d
250                ));
251                output.set_payload(state);
252            }
253            None => output.clear_payload(),
254        };
255        Ok(())
256    }
257
258    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
259        self.pid.reset();
260        self.first_run = true;
261        Ok(())
262    }
263}
264
265/// Store/Restore the internal state of the PID controller.
266impl<I> Freezable for GenericPIDTask<I>
267where
268    f32: for<'a> From<&'a I>,
269{
270    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
271        Encode::encode(&self.pid.integral, encoder)?;
272        Encode::encode(&self.pid.last_error, encoder)?;
273        Encode::encode(&self.pid.elapsed, encoder)?;
274        Encode::encode(&self.pid.last_output, encoder)?;
275        Ok(())
276    }
277
278    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
279        self.pid.integral = Decode::decode(decoder)?;
280        self.pid.last_error = Decode::decode(decoder)?;
281        self.pid.elapsed = Decode::decode(decoder)?;
282        self.pid.last_output = Decode::decode(decoder)?;
283        Ok(())
284    }
285}
286
287// Small helper befause we do this again and again
288fn getcfg(config: &ComponentConfig, key: &str, default: f32) -> f32 {
289    if let Some(value) = config.get::<f64>(key) {
290        value as f32
291    } else {
292        default
293    }
294}
cu_pid/lib.rs

cu_pid/
lib.rs
