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::{Deserialize, 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, Deserialize)]
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 reset_integral(&mut self) {
85        self.integral = 0.0f32;
86    }
87
88    pub fn init_measurement(&mut self, measurement: f32) {
89        self.last_error = self.setpoint - measurement;
90        self.elapsed = self.sampling; // force the computation on the first next_control_output
91    }
92
93    pub fn next_control_output(
94        &mut self,
95        measurement: f32,
96        dt: CuDuration,
97    ) -> PIDControlOutputPayload {
98        self.elapsed += dt;
99
100        if self.elapsed < self.sampling {
101            // if we bang too fast the PID controller, just keep on giving the same answer
102            return self.last_output.clone();
103        }
104
105        let error = self.setpoint - measurement;
106        let CuDuration(elapsed) = self.elapsed;
107        let dt = elapsed as f32 / 1_000_000f32; // the unit is kind of arbitrary.
108
109        // Proportional term
110        let p_unbounded = self.kp * error;
111        let p = p_unbounded.clamp(-self.p_limit, self.p_limit);
112
113        // Integral term (accumulated over time)
114        self.integral += error * dt;
115        let i_unbounded = self.ki * self.integral;
116        let i = i_unbounded.clamp(-self.i_limit, self.i_limit);
117
118        // Derivative term (rate of change)
119        let derivative = (error - self.last_error) / dt;
120        let d_unbounded = self.kd * derivative;
121        let d = d_unbounded.clamp(-self.d_limit, self.d_limit);
122
123        // Update last error for next calculation
124        self.last_error = error;
125
126        // Final output: sum of P, I, D with output limit
127        let output_unbounded = p + i + d;
128        let output = output_unbounded.clamp(-self.output_limit, self.output_limit);
129
130        let output = PIDControlOutputPayload { p, i, d, output };
131
132        self.last_output = output.clone();
133        self.elapsed = CuDuration::default();
134        output
135    }
136}
137
138/// This is the Copper task encapsulating the PID controller.
139pub struct GenericPIDTask<I>
140where
141    f32: for<'a> From<&'a I>,
142{
143    _marker: PhantomData<I>,
144    pid: PIDController,
145    first_run: bool,
146    last_tov: CuTime,
147    setpoint: f32,
148    cutoff: f32,
149}
150
151impl<I> CuTask for GenericPIDTask<I>
152where
153    f32: for<'a> From<&'a I>,
154    I: CuMsgPayload,
155{
156    type Resources<'r> = ();
157    type Input<'m> = input_msg!(I);
158    type Output<'m> = output_msg!(PIDControlOutputPayload);
159
160    fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
161    where
162        Self: Sized,
163    {
164        match config {
165            Some(config) => {
166                debug!("PIDTask config loaded");
167                let setpoint: f32 = config
168                    .get::<f64>("setpoint")
169                    .ok_or("'setpoint' not found in config")?
170                    as f32;
171
172                let cutoff: f32 = config.get::<f64>("cutoff").ok_or(
173                    "'cutoff' not found in config, please set an operating +/- limit on the input.",
174                )? as f32;
175
176                // p is mandatory
177                let kp = if let Some(kp) = config.get::<f64>("kp") {
178                    Ok(kp as f32)
179                } else {
180                    Err(CuError::from(
181                        "'kp' not found in the config. We need at least 'kp' to make the PID algorithm work.",
182                    ))
183                }?;
184
185                let p_limit = getcfg(config, "pl", 2.0f32);
186                let ki = getcfg(config, "ki", 0.0f32);
187                let i_limit = getcfg(config, "il", 1.0f32);
188                let kd = getcfg(config, "kd", 0.0f32);
189                let d_limit = getcfg(config, "dl", 2.0f32);
190                let output_limit = getcfg(config, "ol", 1.0f32);
191
192                let sampling = if let Some(value) = config.get::<u32>("sampling_ms") {
193                    CuDuration::from(value as u64 * 1_000_000u64)
194                } else {
195                    CuDuration::default()
196                };
197
198                let pid: PIDController = PIDController::new(
199                    kp,
200                    ki,
201                    kd,
202                    setpoint,
203                    p_limit,
204                    i_limit,
205                    d_limit,
206                    output_limit,
207                    sampling,
208                );
209
210                Ok(Self {
211                    _marker: PhantomData,
212                    pid,
213                    first_run: true,
214                    last_tov: CuTime::default(),
215                    setpoint,
216                    cutoff,
217                })
218            }
219            None => Err(CuError::from("PIDTask needs a config.")),
220        }
221    }
222
223    fn process(
224        &mut self,
225        _clock: &RobotClock,
226        input: &Self::Input<'_>,
227        output: &mut Self::Output<'_>,
228    ) -> CuResult<()> {
229        match input.payload() {
230            Some(payload) => {
231                let tov = match input.tov {
232                    Tov::Time(single) => single,
233                    _ => return Err("Unexpected variant for a TOV of PID".into()),
234                };
235
236                let measure: f32 = payload.into();
237
238                if self.first_run {
239                    self.first_run = false;
240                    self.last_tov = tov;
241                    self.pid.init_measurement(measure);
242                    output.clear_payload();
243                    return Ok(());
244                }
245                let dt = tov - self.last_tov;
246                self.last_tov = tov;
247
248                // update the status of the pid.
249                let state = self.pid.next_control_output(measure, dt);
250                // But safety check if the input is within operational margins and cut power if it is not.
251                if measure > self.setpoint + self.cutoff {
252                    return Err(
253                        format!("{} > {} (cutoff)", measure, self.setpoint + self.cutoff).into(),
254                    );
255                }
256                if measure < self.setpoint - self.cutoff {
257                    return Err(
258                        format!("{} < {} (cutoff)", measure, self.setpoint - self.cutoff).into(),
259                    );
260                }
261                output.metadata.set_status(format!(
262                    "{:>5.2} {:>5.2} {:>5.2} {:>5.2}",
263                    &state.output, &state.p, &state.i, &state.d
264                ));
265                output.set_payload(state);
266            }
267            None => output.clear_payload(),
268        };
269        Ok(())
270    }
271
272    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
273        self.pid.reset();
274        self.first_run = true;
275        Ok(())
276    }
277}
278
279/// Store/Restore the internal state of the PID controller.
280impl<I> Freezable for GenericPIDTask<I>
281where
282    f32: for<'a> From<&'a I>,
283{
284    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
285        Encode::encode(&self.pid.integral, encoder)?;
286        Encode::encode(&self.pid.last_error, encoder)?;
287        Encode::encode(&self.pid.elapsed, encoder)?;
288        Encode::encode(&self.pid.last_output, encoder)?;
289        Ok(())
290    }
291
292    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
293        self.pid.integral = Decode::decode(decoder)?;
294        self.pid.last_error = Decode::decode(decoder)?;
295        self.pid.elapsed = Decode::decode(decoder)?;
296        self.pid.last_output = Decode::decode(decoder)?;
297        Ok(())
298    }
299}
300
301// Small helper befause we do this again and again
302fn getcfg(config: &ComponentConfig, key: &str, default: f32) -> f32 {
303    if let Some(value) = config.get::<f64>(key) {
304        value as f32
305    } else {
306        default
307    }
308}
cu_pid/lib.rs

cu_pid/
lib.rs
