cu_pid/
lib.rs

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

cu_pid/
lib.rs
