diff --git a/Cargo.toml b/Cargo.toml index 6338a66..11aa3df 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "pid" -version = "3.0.0" +version = "4.0.0" edition = "2018" authors = ["Ken Elkabany "] license = "MIT" diff --git a/README.md b/README.md index d269b54..0975166 100644 --- a/README.md +++ b/README.md @@ -30,46 +30,42 @@ A proportional-integral-derivative (PID) controller. ## Example ```rust -extern crate pid; use pid::Pid; -fn main() { - // Set only kp (proportional) to 10. The setpoint is 15. - // Set limits for P, I, and D to 100 each. - let mut pid = Pid::new(10.0, 0.0, 0.0, 100.0, 100.0, 100.0, 100.0, 15.0); - // Fake a measurement of 10.0, which is an error of 5.0. - let output = pid.next_control_output(10.0); - // Verify that kp * error = 10.0 * 5.0 = 50.0 - assert_eq!(output.output, 50.0); - // Verify that all output was from the proportional term - assert_eq!(output.p, 50.0); - assert_eq!(output.i, 0.0); - assert_eq!(output.d, 0.0); - - // Verify that the same measurement produces the same output since we - // aren't using the stateful derivative & integral terms. - let output = pid.next_control_output(10.0); - assert_eq!(output.p, 50.0); - - // Add an integral term - pid.ki = 1.0; - let output = pid.next_control_output(10.0); - assert_eq!(output.p, 50.0); - // Verify that the integral term is adding to the output signal. - assert_eq!(output.i, 5.0); - assert_eq!(output.output, 55.0); - - // Add a derivative term - pid.kd = 2.0; - let output = pid.next_control_output(15.0); // Match the desired target - // No proportional term since no error - assert_eq!(output.p, 0.0); - // Integral term stays the same - assert_eq!(output.i, 5.0); - // Derivative on measurement produces opposing signal - assert_eq!(output.d, -10.0); - assert_eq!(output.output, -5.0); -} +// Create a new proportional-only PID controller with a setpoint of 15 +let mut pid = Pid::new(15.0, 100.0); +pid.p(10.0, 100.0); + +// Input a mesurement with an error of 5.0 from our setpoint +let output = pid.next_control_output(10.0); + +// Show that the error is correct by multiplying by our kp +assert_eq!(output.output, 50.0); // <-- +assert_eq!(output.p, 50.0); + +// It won't change on repeat; the controller is proportional-only +let output = pid.next_control_output(10.0); +assert_eq!(output.output, 50.0); // <-- +assert_eq!(output.p, 50.0); + +// Add a new integral term to the controller and input again +pid.i(1.0, 100.0); +let output = pid.next_control_output(10.0); + +// Now that the integral makes the controller stateful, it will change +assert_eq!(output.output, 55.0); // <-- +assert_eq!(output.p, 50.0); +assert_eq!(output.i, 5.0); + +// Add our final derivative term and match our setpoint target +pid.d(2.0, 100.0); +let output = pid.next_control_output(15.0); + +// The output will now say to go down due to the derivative +assert_eq!(output.output, -5.0); // <-- +assert_eq!(output.p, 0.0); +assert_eq!(output.i, 5.0); +assert_eq!(output.d, -10.0); ``` ## Assumptions diff --git a/src/lib.rs b/src/lib.rs index f73b96d..5e49980 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,40 +1,119 @@ -//! A proportional-integral-derivative (PID) controller. +//! A proportional-integral-derivative (PID) controller library. +//! +//! See [Pid] for the adjustable controller itself, as well as [ControlOutput] for the outputs and weights which you can use after setting up your controller. Follow the complete example below to setup your first controller! +//! +//! # Example +//! +//! ```rust +//! use pid::Pid; +//! +//! // Create a new proportional-only PID controller with a setpoint of 15 +//! let mut pid = Pid::new(15.0, 100.0); +//! pid.p(10.0, 100.0); +//! +//! // Input a mesurement with an error of 5.0 from our setpoint +//! let output = pid.next_control_output(10.0); +//! +//! // Show that the error is correct by multiplying by our kp +//! assert_eq!(output.output, 50.0); // <-- +//! assert_eq!(output.p, 50.0); +//! +//! // It won't change on repeat; the controller is proportional-only +//! let output = pid.next_control_output(10.0); +//! assert_eq!(output.output, 50.0); // <-- +//! assert_eq!(output.p, 50.0); +//! +//! // Add a new integral term to the controller and input again +//! pid.i(1.0, 100.0); +//! let output = pid.next_control_output(10.0); +//! +//! // Now that the integral makes the controller stateful, it will change +//! assert_eq!(output.output, 55.0); // <-- +//! assert_eq!(output.p, 50.0); +//! assert_eq!(output.i, 5.0); +//! +//! // Add our final derivative term and match our setpoint target +//! pid.d(2.0, 100.0); +//! let output = pid.next_control_output(15.0); +//! +//! // The output will now say to go down due to the derivative +//! assert_eq!(output.output, -5.0); // <-- +//! assert_eq!(output.p, 0.0); +//! assert_eq!(output.i, 5.0); +//! assert_eq!(output.d, -10.0); +//! ``` #![no_std] use num_traits::float::FloatCore; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +/// Adjustable proportional-integral-derivative (PID) controller. +/// +/// # Examples +/// +/// This controller provides a builder pattern interface which allows you to pick-and-choose which PID inputs you'd like to use during operation. Here's what a basic proportional-only controller could look like: +/// +/// ```rust +/// use pid::Pid; +/// +/// // Create limited controller +/// let mut p_controller = Pid::new(15.0, 100.0); +/// p_controller.p(10.0, 100.0); +/// +/// // Get first output +/// let p_output = p_controller.next_control_output(400.0); +/// ``` +/// +/// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: +/// +/// ```rust +/// use pid::Pid; +/// +/// // Create full PID controler +/// let mut full_controller = Pid::new(15.0, 100.0); +/// full_controller.p(10.0, 100.0).i(4.5, 100.0).d(0.25, 100.0); +/// +/// // Get first output +/// let full_output = full_controller.next_control_output(400.0); +/// ``` +/// +/// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. +/// +/// The last item of note is that these [`p`](Self::p()), [`i`](Self::i()), and [`d`](Self::d()) methods can be used *during* operation which lets you add and/or modify these controller values if need be. #[derive(Clone, Copy, Debug, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] pub struct Pid { + /// Ideal setpoint to strive for. + pub setpoint: T, + /// Defines the overall output filter limit. + pub output_limit: T, /// Proportional gain. pub kp: T, /// Integral gain. pub ki: T, /// Derivative gain. pub kd: T, - /// Limit of contribution of P term: `(-p_limit <= P <= p_limit)` + /// Limiter for the proportional term: `(-p_limit <= P <= p_limit)`. pub p_limit: T, - /// Limit of contribution of I term `(-i_limit <= I <= i_limit)` + /// Limiter for the integral term: `(-i_limit <= I <= i_limit)`. pub i_limit: T, - /// Limit of contribution of D term `(-d_limit <= D <= d_limit)` + /// Limiter for the derivative term: `(-d_limit <= D <= d_limit)`. pub d_limit: T, - /// Limit of output `(-output_limit <= output <= output_limit)` - pub output_limit: T, - - pub setpoint: T, - prev_measurement: Option, - /// `integral_term = sum[error(t) * ki(t)] (for all t)` + /// Last calculated integral value if [Pid::ki] is used. integral_term: T, + /// Previously found measurement whilst using the [Pid::next_control_output] method. + prev_measurement: Option, } -#[derive(Debug)] +/// Output of [controller iterations](Pid::next_control_output) with weights +#[derive(Debug, PartialEq, Eq)] pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. - /// `i = sum[error(t) * ki(t)] (for all t)` + /// + /// This integral term is equal to `sum[error(t) * ki(t)] (for all t)` pub i: T, /// Contribution of the D term to the output. pub d: T, @@ -46,40 +125,59 @@ impl Pid where T: FloatCore, { - pub fn new( - kp: T, - ki: T, - kd: T, - p_limit: T, - i_limit: T, - d_limit: T, - output_limit: T, - setpoint: T, - ) -> Self { + /// Creates a new controller with the target setpoint and the output limit + /// + /// To set your P, I, and D terms into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional term setting + /// - [Self::i()]: Integral term setting + /// - [Self::d()]: Derivative term setting + pub fn new(setpoint: impl Into, output_limit: impl Into) -> Self { Self { - kp, - ki, - kd, - p_limit, - i_limit, - d_limit, - output_limit, - setpoint, - prev_measurement: None, + setpoint: setpoint.into(), + output_limit: output_limit.into(), + kp: T::zero(), + ki: T::zero(), + kd: T::zero(), + p_limit: T::zero(), + i_limit: T::zero(), + d_limit: T::zero(), integral_term: T::zero(), + prev_measurement: None, } } - /// Resets the integral term back to zero. This may drastically change the - /// control output. - pub fn reset_integral_term(&mut self) { - self.integral_term = T::zero(); + /// Sets the [Self::p] term for this controller. + pub fn p(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { + self.kp = gain.into(); + self.p_limit = limit.into(); + self + } + + /// Sets the [Self::i] term for this controller. + pub fn i(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { + self.ki = gain.into(); + self.i_limit = limit.into(); + self + } + + /// Sets the [Self::d] term for this controller. + pub fn d(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { + self.kd = gain.into(); + self.d_limit = limit.into(); + self + } + + /// Sets the [Pid::setpoint] to target for this controller. + pub fn setpoint(&mut self, setpoint: impl Into) -> &mut Self { + self.setpoint = setpoint.into(); + self } /// Given a new measurement, calculates the next control output. /// /// # Panics - /// If a setpoint has not been set via `update_setpoint()`. + /// + /// - If a setpoint has not been set via `update_setpoint()`. pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { let error = self.setpoint - measurement; @@ -115,6 +213,12 @@ where output: output, } } + + /// Resets the integral term back to zero, this may drastically change the + /// control output. + pub fn reset_integral_term(&mut self) { + self.integral_term = T::zero(); + } } /// Saturating the input `value` according the absolute `limit` (`-limit <= output <= limit`). @@ -124,11 +228,14 @@ fn apply_limit(limit: T, value: T) -> T { #[cfg(test)] mod tests { + use crate::ControlOutput; use super::Pid; + /// Proportional-only controller operation and limits #[test] fn proportional() { - let mut pid = Pid::new(2.0, 0.0, 0.0, 100.0, 100.0, 100.0, 100.0, 10.0); + let mut pid = Pid::new(10.0, 100.0); + pid.p(2.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); assert_eq!(pid.setpoint, 10.0); // Test simple proportional @@ -139,9 +246,11 @@ mod tests { assert_eq!(pid.next_control_output(0.0).output, 10.0); } + /// Derivative-only controller operation and limits #[test] fn derivative() { - let mut pid = Pid::new(0.0, 0.0, 2.0, 100.0, 100.0, 100.0, 100.0, 10.0); + let mut pid = Pid::new(10.0, 100.0); + pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement assert_eq!(pid.next_control_output(0.0).output, 0.0); @@ -154,9 +263,11 @@ mod tests { assert_eq!(pid.next_control_output(10.0).output, -5.0); } + /// Integral-only controller operation and limits #[test] fn integral() { - let mut pid = Pid::new(0.0, 2.0, 0.0, 100.0, 100.0, 100.0, 100.0, 10.0); + let mut pid = Pid::new(10.0, 100.0); + pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration assert_eq!(pid.next_control_output(0.0).output, 20.0); @@ -170,7 +281,8 @@ mod tests { assert_eq!(pid.next_control_output(15.0).output, 40.0); // Test that error integral accumulates negative values - let mut pid2 = Pid::new(0.0, 2.0, 0.0, 100.0, 100.0, 100.0, 100.0, -10.0); + let mut pid2 = Pid::new(-10.0, 100.0); + pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); assert_eq!(pid2.next_control_output(0.0).output, -20.0); assert_eq!(pid2.next_control_output(0.0).output, -40.0); @@ -180,9 +292,11 @@ mod tests { assert_eq!(pid2.next_control_output(-15.0).output, -40.0); } + /// Checks that a full PID controller's limits work properly through multiple output iterations #[test] fn output_limit() { - let mut pid = Pid::new(1.0, 0.0, 0.0, 100.0, 100.0, 100.0, 1.0, 10.0); + let mut pid = Pid::new(10.0, 1.0); + pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 @@ -193,9 +307,11 @@ mod tests { assert_eq!(out.output, -1.0); } + /// Combined PID operation #[test] fn pid() { - let mut pid = Pid::new(1.0, 0.1, 1.0, 100.0, 100.0, 100.0, 100.0, 10.0); + let mut pid = Pid::new(10.0, 100.0); + pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 @@ -222,11 +338,14 @@ mod tests { assert_eq!(out.output, 2.4); } + /// Full PID operation with mixed f32/f64 checking to make sure they're equal #[test] fn f32_and_f64() { - let mut pid32 = Pid::new(2.0f32, 0.0, 0.0, 100.0, 100.0, 100.0, 100.0, 10.0); + let mut pid32 = Pid::new(10.0f32, 100.0); + pid32.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let mut pid64 = Pid::new(2.0f64, 0.0, 0.0, 100.0, 100.0, 100.0, 100.0, 10.0); + let mut pid64 = Pid::new(10.0, 100.0f64); + pid64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); assert_eq!( pid32.next_control_output(0.0).output, @@ -237,4 +356,29 @@ mod tests { pid64.next_control_output(0.0).output ); } + + /// See if the controller can properly target to the setpoint after 2 output iterations + #[test] + fn setpoint() { + let mut pid = Pid::new(10.0, 100.0); + pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); + + let out = pid.next_control_output(0.0); + assert_eq!(out.p, 10.0); // 1.0 * 10.0 + assert_eq!(out.i, 1.0); // 0.1 * 10.0 + assert_eq!(out.d, 0.0); // -(1.0 * 0.0) + assert_eq!(out.output, 11.0); + + pid.setpoint(0.0); + + assert_eq!( + pid.next_control_output(0.0), + ControlOutput { + p: 0.0, + i: 1.0, + d: -0.0, + output: 1.0 + } + ); + } }