BFPOWER/physical
2
0
Fork 0
Code Issues 2 Pull Requests Releases Activity

Removed builder methods, easier to just set fields directly.

Browse Source
This commit is contained in:
Zachary Sunforge
2024-06-04 23:05:51 -07:00
parent 46abb2150c
commit d37d99537e
1 changed files with 81 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

175
src/control/pid.rs
View File

@ -9,7 +9,8 @@
//! //!
//! // Create a new proportional-only PID controller with a setpoint of 15 //! // Create a new proportional-only PID controller with a setpoint of 15
//! let mut pid = Pid::new(15.0, 100.0); //! let mut pid = Pid::new(15.0, 100.0);
//! pid.p(10.0, 100.0); //! pid.proportional_gain = 10.0;
//! pid.proportional_limit = 100.0;
//! //!
//! // Input a measurement with an error of 5.0 from our setpoint //! // Input a measurement with an error of 5.0 from our setpoint
//! let output = pid.next_control_output(10.0); //! let output = pid.next_control_output(10.0);
@ -22,14 +23,16 @@
//! assert_eq!(output, 50.0); // <-- //! assert_eq!(output, 50.0); // <--
//! //!
//! // Add a new integral term to the controller and input again //! // Add a new integral term to the controller and input again
//! pid.i(1.0, 100.0); //! pid.integral_gain = 1.0;
//! pid.integral_limit = 100.0;
//! let output = pid.next_control_output(10.0); //! let output = pid.next_control_output(10.0);
//! //!
//! // Now that the integral makes the controller stateful, it will change //! // Now that the integral makes the controller stateful, it will change
//! assert_eq!(output, 55.0); // <-- //! assert_eq!(output, 55.0); // <--
//! //!
//! // Add our final derivative term and match our setpoint target //! // Add our final derivative term and match our setpoint target
//! pid.d(2.0, 100.0); //! pid.derivative_gain = 2.0;
//! pid.derivative_limit = 100.0;
//! let output = pid.next_control_output(15.0); //! let output = pid.next_control_output(15.0);
//! //!
//! // The output will now say to go down due to the derivative //! // The output will now say to go down due to the derivative
@ -58,40 +61,11 @@ impl<T: PartialOrd + num_traits::Signed + Copy> Number for T {}
/// Adjustable proportional-integral-derivative (PID) controller. /// 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 physical::control::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 physical::control::pid::Pid;
///
/// // Create full PID controller
/// 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. /// 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. /// The last item of note is that the gain and limit fields can be safely modified during use.
/// ///
/// # Type Warning /// # Type Warning
///
/// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller.
#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] #[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)]
#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
@ -102,19 +76,19 @@ pub struct Pid<T: Number> {
pub output_limit: T, pub output_limit: T,
/// Proportional gain. The proportional component is dependant only on the error. /// Proportional gain. The proportional component is dependant only on the error.
/// It is the error * this value. /// It is the error * this value.
pub p_gain: T, pub proportional_gain: T,
/// Integral gain. The integral component is dependent on the error and the integral term from the previous iteration. /// Integral gain. The integral component is dependent on the error and the integral term from the previous iteration.
/// It is the previous integral + (error * this value). /// It is the previous integral + (error * this value).
pub i_gain: T, pub integral_gain: T,
/// Derivative gain. The derivative component is dependent on the rate of change of the measurement. /// Derivative gain. The derivative component is dependent on the rate of change of the measurement.
/// It is the (current measurement - previous measurement) * this value. /// It is the (current measurement - previous measurement) * this value.
pub d_gain: T, pub derivative_gain: T,
/// Limiter for the proportional term: `-p_limit <= P <= p_limit`. /// Limiter for the proportional term: `-p_limit <= P <= p_limit`.
pub p_limit: T, pub proportional_limit: T,
/// Limiter for the integral term: `-i_limit <= I <= i_limit`. /// Limiter for the integral term: `-i_limit <= I <= i_limit`.
pub i_limit: T, pub integral_limit: T,
/// Limiter for the derivative term: `-d_limit <= D <= d_limit`. /// Limiter for the derivative term: `-d_limit <= D <= d_limit`.
pub d_limit: T, pub derivative_limit: T,
/// Last calculated integral value if [Pid::i_gain] is used. /// Last calculated integral value if [Pid::i_gain] is used.
integral_term: T, integral_term: T,
/// Previously found measurement whilst using the [Pid::next_control_output] method. /// Previously found measurement whilst using the [Pid::next_control_output] method.
@ -126,47 +100,21 @@ where
T: Number, T: Number,
{ {
/// Creates a new controller with the target setpoint and the output limit /// 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: T, output_limit: T) -> Self { pub fn new(setpoint: T, output_limit: T) -> Self {
Self { Self {
setpoint, setpoint,
output_limit, output_limit,
p_gain: T::zero(), proportional_gain: T::zero(),
i_gain: T::zero(), integral_gain: T::zero(),
d_gain: T::zero(), derivative_gain: T::zero(),
p_limit: T::zero(), proportional_limit: T::zero(),
i_limit: T::zero(), integral_limit: T::zero(),
d_limit: T::zero(), derivative_limit: T::zero(),
integral_term: T::zero(), integral_term: T::zero(),
prev_measurement: None, prev_measurement: None,
} }
} }
/// Sets the [Self::p] term for this controller.
pub fn p(&mut self, gain: T, limit: T) -> &mut Self {
self.p_gain = gain;
self.p_limit = limit;
self
}
/// Sets the [Self::i] term for this controller.
pub fn i(&mut self, gain: T, limit: T) -> &mut Self {
self.i_gain = gain;
self.i_limit = limit;
self
}
/// Sets the [Self::d] term for this controller.
pub fn d(&mut self, gain: T, limit: T) -> &mut Self {
self.d_gain = gain;
self.d_limit = limit;
self
}
/// Sets the [Pid::setpoint] to target for this controller. /// Sets the [Pid::setpoint] to target for this controller.
pub fn setpoint(&mut self, setpoint: T) -> &mut Self { pub fn setpoint(&mut self, setpoint: T) -> &mut Self {
self.setpoint = setpoint; self.setpoint = setpoint;
@ -180,28 +128,28 @@ where
let error = self.setpoint - measurement; let error = self.setpoint - measurement;
// Calculate the proportional term and limit to it's individual limit // Calculate the proportional term and limit to it's individual limit
let p_unbounded = error * self.p_gain; let p_unbounded = error * self.proportional_gain;
let p = apply_limit(self.p_limit, p_unbounded); let p = apply_limit(self.proportional_limit, p_unbounded);
// Mitigate output jumps when ki(t) != ki(t-1). // Mitigate output jumps when ki(t) != ki(t-1).
// While it's standard to use an error_integral that's a running sum of // While it's standard to use an error_integral that's a running sum of
// just the error (no ki), because we support ki changing dynamically, // just the error (no ki), because we support ki changing dynamically,
// we store the entire term so that we don't need to remember previous // we store the entire term so that we don't need to remember previous
// ki values. // ki values.
self.integral_term = self.integral_term + error * self.i_gain; self.integral_term = self.integral_term + error * self.integral_gain;
// Mitigate integral windup: Don't want to keep building up error // Mitigate integral windup: Don't want to keep building up error
// beyond what i_limit will allow. // beyond what i_limit will allow.
self.integral_term = apply_limit(self.i_limit, self.integral_term); self.integral_term = apply_limit(self.integral_limit, self.integral_term);
// Mitigate derivative kick: Use the derivative of the measurement // Mitigate derivative kick: Use the derivative of the measurement
// rather than the derivative of the error. // rather than the derivative of the error.
let d_unbounded = -match self.prev_measurement { let d_unbounded = -match self.prev_measurement {
Some(prev_measurement) => measurement - prev_measurement, Some(prev_measurement) => measurement - prev_measurement,
None => T::zero(), None => T::zero(),
} * self.d_gain; } * self.derivative_gain;
self.prev_measurement = Some(measurement); self.prev_measurement = Some(measurement);
let d = apply_limit(self.d_limit, d_unbounded); let d = apply_limit(self.derivative_limit, d_unbounded);
// Calculate the final output by adding together the PID terms, then // Calculate the final output by adding together the PID terms, then
// apply the final defined output limit // apply the final defined output limit
@ -231,14 +179,17 @@ mod tests {
#[test] #[test]
fn proportional() { fn proportional() {
let mut pid = Pid::new(10.0, 100.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); pid.proportional_gain = 2.0;
pid.proportional_limit = 100.0;
pid.integral_limit = 100.0;
pid.derivative_limit = 100.0;
assert_eq!(pid.setpoint, 10.0); assert_eq!(pid.setpoint, 10.0);
// Test simple proportional // Test simple proportional
assert_eq!(pid.next_control_output(0.0), 20.0); assert_eq!(pid.next_control_output(0.0), 20.0);
// Test proportional limit // Test proportional limit
pid.p_limit = 10.0; pid.proportional_limit = 10.0;
assert_eq!(pid.next_control_output(0.0), 10.0); assert_eq!(pid.next_control_output(0.0), 10.0);
} }
@ -246,7 +197,10 @@ mod tests {
#[test] #[test]
fn derivative() { fn derivative() {
let mut pid = Pid::new(10.0, 100.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); pid.proportional_limit = 100.0;
pid.integral_limit = 100.0;
pid.derivative_limit = 100.0;
pid.derivative_gain = 2.0;
// Test that there's no derivative since it's the first measurement // Test that there's no derivative since it's the first measurement
assert_eq!(pid.next_control_output(0.0), 0.0); assert_eq!(pid.next_control_output(0.0), 0.0);
@ -255,7 +209,7 @@ mod tests {
assert_eq!(pid.next_control_output(5.0), -10.0); assert_eq!(pid.next_control_output(5.0), -10.0);
// Test derivative limit // Test derivative limit
pid.d_limit = 5.0; pid.derivative_limit = 5.0;
assert_eq!(pid.next_control_output(10.0), -5.0); assert_eq!(pid.next_control_output(10.0), -5.0);
} }
@ -263,7 +217,10 @@ mod tests {
#[test] #[test]
fn integral() { fn integral() {
let mut pid = Pid::new(10.0, 100.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); pid.proportional_limit = 0.0;
pid.integral_gain = 2.0;
pid.integral_limit = 100.0;
pid.derivative_limit = 100.0;
// Test basic integration // Test basic integration
assert_eq!(pid.next_control_output(0.0), 20.0); assert_eq!(pid.next_control_output(0.0), 20.0);
@ -271,18 +228,22 @@ mod tests {
assert_eq!(pid.next_control_output(5.0), 50.0); assert_eq!(pid.next_control_output(5.0), 50.0);
// Test limit // Test limit
pid.i_limit = 50.0; pid.integral_limit = 50.0;
assert_eq!(pid.next_control_output(5.0), 50.0); assert_eq!(pid.next_control_output(5.0), 50.0);
// Test that limit doesn't impede reversal of error integral // Test that limit doesn't impede reversal of error integral
assert_eq!(pid.next_control_output(15.0), 40.0); assert_eq!(pid.next_control_output(15.0), 40.0);
// Test that error integral accumulates negative values // Test that error integral accumulates negative values
let mut pid2 = Pid::new(-10.0, 100.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); pid2.proportional_limit = 100.0;
pid2.integral_gain = 2.0;
pid2.integral_limit = 100.0;
pid2.derivative_limit = 100.0;
assert_eq!(pid2.next_control_output(0.0), -20.0); assert_eq!(pid2.next_control_output(0.0), -20.0);
assert_eq!(pid2.next_control_output(0.0), -40.0); assert_eq!(pid2.next_control_output(0.0), -40.0);
pid2.i_limit = 50.0; pid2.integral_limit = 50.0;
assert_eq!(pid2.next_control_output(-5.0), -50.0); assert_eq!(pid2.next_control_output(-5.0), -50.0);
// Test that limit doesn't impede reversal of error integral // Test that limit doesn't impede reversal of error integral
assert_eq!(pid2.next_control_output(-15.0), -40.0); assert_eq!(pid2.next_control_output(-15.0), -40.0);
@ -292,7 +253,10 @@ mod tests {
#[test] #[test]
fn output_limit() { fn output_limit() {
let mut pid = Pid::new(10.0, 1.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); pid.proportional_gain = 2.0;
pid.proportional_limit = 100.0;
pid.integral_limit = 100.0;
pid.derivative_limit = 100.0;
let out = pid.next_control_output(0.0); let out = pid.next_control_output(0.0);
assert_eq!(out, 1.0); assert_eq!(out, 1.0);
@ -305,7 +269,12 @@ mod tests {
#[test] #[test]
fn pid() { fn pid() {
let mut pid = Pid::new(10.0, 100.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); pid.proportional_gain = 1.0;
pid.proportional_limit = 100.0;
pid.integral_gain = 0.1;
pid.integral_limit = 100.0;
pid.derivative_gain = 1.0;
pid.derivative_limit = 100.0;
let out = pid.next_control_output(0.0); let out = pid.next_control_output(0.0);
assert_eq!(out, 11.0); assert_eq!(out, 11.0);
@ -325,10 +294,14 @@ mod tests {
#[test] #[test]
fn floats_zeros() { fn floats_zeros() {
let mut pid_f32 = Pid::new(10.0f32, 100.0); let mut pid_f32 = Pid::new(10.0f32, 100.0);
pid_f32.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); pid_f32.proportional_limit = 100.0;
pid_f32.integral_limit = 100.0;
pid_f32.derivative_limit = 100.0;
let mut pid_f64 = Pid::new(10.0, 100.0f64); let mut pid_f64 = Pid::new(10.0, 100.0f64);
pid_f64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); pid_f64.proportional_limit = 100.0;
pid_f64.integral_limit = 100.0;
pid_f64.derivative_limit = 100.0;
for _ in 0..5 { for _ in 0..5 {
assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32); assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32);
@ -340,10 +313,14 @@ mod tests {
#[test] #[test]
fn signed_integers_zeros() { fn signed_integers_zeros() {
let mut pid_i8 = Pid::new(10i8, 100); let mut pid_i8 = Pid::new(10i8, 100);
pid_i8.p(0, 100).i(0, 100).d(0, 100); pid_i8.proportional_limit = 100;
pid_i8.integral_limit = 100;
pid_i8.derivative_limit = 100;
let mut pid_i32 = Pid::new(10i32, 100); let mut pid_i32 = Pid::new(10i32, 100);
pid_i32.p(0, 100).i(0, 100).d(0, 100); pid_i32.proportional_limit = 100;
pid_i32.integral_limit = 100;
pid_i32.derivative_limit = 100;
for _ in 0..5 { for _ in 0..5 {
assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32); assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32);
@ -354,7 +331,12 @@ mod tests {
#[test] #[test]
fn setpoint() { fn setpoint() {
let mut pid = Pid::new(10.0, 100.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); pid.proportional_gain = 1.0;
pid.proportional_limit = 100.0;
pid.integral_gain = 0.1;
pid.integral_limit = 100.0;
pid.derivative_gain = 1.0;
pid.derivative_limit = 100.0;
let out = pid.next_control_output(0.0); let out = pid.next_control_output(0.0);
assert_eq!(out, 11.0); assert_eq!(out, 11.0);
@ -368,7 +350,12 @@ mod tests {
#[test] #[test]
fn negative_limits() { fn negative_limits() {
let mut pid = Pid::new(10.0f32, -10.0); let mut pid = Pid::new(10.0f32, -10.0);
pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); pid.proportional_gain = 1.0;
pid.proportional_limit = -50.0;
pid.integral_gain = 1.0;
pid.integral_limit = -50.0;
pid.derivative_gain = 1.0;
pid.derivative_limit = -50.0;
let out = pid.next_control_output(0.0); let out = pid.next_control_output(0.0);
assert_eq!(out, 10.0); assert_eq!(out, 10.0);