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

cu_pid/
lib.rs
