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

cu_pid/
lib.rs
