pid_loop/
lib.rs

1//! A tiny `no_std` PID controller library.
2//!
3//! This crate implements the classic windowed PID loop over abstract data type.
4//!
5//! # Introduction
6//!
7//! A proportional-integral-derivative controller is a control loop mechanism
8//! employing feedback that is widely used in industrial control systems
9//! and a variety of other applications requiring continuously modulated control.
10//!
11//! # Examples
12//!
13//! ```no_run
14//! use pid_loop::PID;
15//!
16//! let target = 42.0;
17//! let mut controller = PID::<f32, 1>::new(2.0, 0.7, 0.3, 0.0, 0.0);
18//! loop {
19//!     let correction = controller.next(target, measure());
20//!     apply_correction(correction);
21//!     sleep();
22//! }
23//!
24//! fn sleep() { todo!() }
25//! fn measure() -> f32 { todo!() }
26//! fn apply_correction(_: f32) { todo!() }
27//! ```
28
29#![no_std]
30#![deny(nonstandard_style, future_incompatible, rust_2018_idioms)]
31use core::ops::*;
32
33/// PID controller
34///
35/// # Examples
36///
37/// ```no_run
38/// use pid_loop::PID;
39///
40/// let target = 42.0;
41/// let mut controller = PID::<f64, 1>::new(2.0, 0.7, 0.3, 0.0, 0.0);
42/// let correction = controller.next(target, measure());
43///
44/// fn measure() -> f64 { todo!() }
45/// ```
46pub struct PID<F, const W: usize> {
47    /// Proportional gain.
48    pub kp: F,
49    /// Integral gain.
50    pub ki: F,
51    /// Derivative gain.
52    pub kd: F,
53    /// Feed forward gain
54    pub kf: F,
55    /// Velocity
56    pub kv: F,
57    last_sp: F,
58    last_error_idx: usize,
59    errors: [F; W],
60    err_history: F,
61}
62
63impl<F, const W: usize> PID<F, W>
64where
65    F: Default + Add<Output = F> + Sub<Output = F> + Mul<Output = F> + PartialOrd + Copy,
66{
67    /// Create a new instance of `PID`.
68    ///
69    /// # Examples
70    ///
71    /// ```
72    /// #![allow(unused_assignments)]
73    /// use pid_loop::PID;
74    ///
75    /// let mut controller = PID::<f32, 1>::new(0.7, 0.034, 0.084, 0.1, 0.0);
76    /// ```
77    pub fn new(
78        kp: impl Into<F>,
79        ki: impl Into<F>,
80        kd: impl Into<F>,
81        kf: impl Into<F>,
82        kv: impl Into<F>,
83    ) -> Self {
84        assert!(W > 0);
85        Self {
86            kp: kp.into(),
87            ki: ki.into(),
88            kd: kd.into(),
89            kf: kf.into(),
90            kv: kv.into(),
91            last_sp: F::default(),
92            errors: [F::default(); W],
93            last_error_idx: usize::default(),
94            err_history: F::default(),
95        }
96    }
97
98    /// Reset controller internal state.
99    ///
100    /// # Examples
101    ///
102    /// ```
103    /// #![allow(unused_assignments)]
104    /// use pid_loop::PID;
105    ///
106    /// let target = 30.0;
107    /// let mut controller = PID::<f32, 1>::new(0.7, 0.034, 0.084, 0.1, 0.0);
108    /// controller.next(target, 42.0);
109    /// controller.reset();
110    ///
111    /// ```
112    pub fn reset(&mut self) {
113        self.last_sp = F::default();
114        self.err_history = F::default();
115        self.errors = [F::default(); W];
116        self.last_error_idx = usize::default();
117    }
118
119    /// Push next measurement into the controller and return correction.
120    ///
121    /// # Examples
122    ///
123    /// ```
124    /// #![allow(unused_assignments)]
125    /// use pid_loop::PID;
126    ///
127    /// let target = 30.0;
128    /// let mut controller = PID::<f64, 1>::new(0.7, 0.034, 0.084, 0.1, 0.1);
129    /// let correction = controller.next(target, 42.0);
130    /// ```
131    pub fn next(&mut self, sp: impl Into<F>, fb: impl Into<F>) -> F {
132        let sp = sp.into();
133        let fb = fb.into();
134        let error = sp - fb;
135
136        let error_delta = error - self.errors[self.last_error_idx];
137        self.push_error(error);
138
139        let sp_delta = sp - self.last_sp;
140        self.last_sp = sp;
141
142        let p = self.kp * error;
143        let i = self.ki * self.err_history;
144        let d = self.kd * error_delta;
145        let f = self.kf * sp_delta;
146        let v = self.kv * fb;
147
148        p + i + d + f + v
149    }
150
151    fn push_error(&mut self, error: F) {
152        self.last_error_idx += 1;
153        if self.last_error_idx >= W {
154            self.last_error_idx = 0;
155        }
156
157        self.err_history = self.err_history - self.errors[self.last_error_idx];
158        self.err_history = self.err_history + error;
159        self.errors[self.last_error_idx] = error;
160    }
161}
pid_loop/lib.rs

pid_loop/
lib.rs
