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

Removed ControlOutput type.

Browse Source
This commit is contained in:
Zachary Sunforge
2024-06-04 22:29:45 -07:00
parent f1f5947f22
commit 46abb2150c
1 changed files with 43 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

161
src/control/pid.rs
View File

@ -15,33 +15,25 @@
//! let output = pid.next_control_output(10.0); //! let output = pid.next_control_output(10.0);
//! //!
//! // Show that the error is correct by multiplying by our kp //! // Show that the error is correct by multiplying by our kp
//! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output, 50.0); // <--
//! assert_eq!(output.p, 50.0);
//! //!
//! // It won't change on repeat; the controller is proportional-only //! // It won't change on repeat; the controller is proportional-only
//! let output = pid.next_control_output(10.0); //! let output = pid.next_control_output(10.0);
//! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output, 50.0); // <--
//! assert_eq!(output.p, 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.i(1.0, 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.output, 55.0); // <-- //! assert_eq!(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 //! // Add our final derivative term and match our setpoint target
//! pid.d(2.0, 100.0); //! pid.d(2.0, 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
//! assert_eq!(output.output, -5.0); // <-- //! assert_eq!(output, -5.0); // <--//!
//! assert_eq!(output.p, 0.0);
//! assert_eq!(output.i, 5.0);
//! assert_eq!(output.d, -10.0);
//!
#[cfg(feature = "serde")] #[cfg(feature = "serde")]
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
@ -108,11 +100,14 @@ pub struct Pid<T: Number> {
pub setpoint: T, pub setpoint: T,
/// Defines the overall output filter limit. /// Defines the overall output filter limit.
pub output_limit: T, pub output_limit: T,
/// Proportional gain. /// Proportional gain. The proportional component is dependant only on the error.
/// It is the error * this value.
pub p_gain: T, pub p_gain: T,
/// Integral gain. /// 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).
pub i_gain: T, pub i_gain: T,
/// Derivative gain. /// Derivative gain. The derivative component is dependent on the rate of change of the measurement.
/// It is the (current measurement - previous measurement) * this value.
pub d_gain: T, pub d_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 p_limit: T,
@ -126,40 +121,9 @@ pub struct Pid<T: Number> {
prev_measurement: Option<T>, prev_measurement: Option<T>,
} }
/// Output of [controller iterations](Pid::next_control_output) with weights
///
/// # Example
///
/// This structure is simple to use and features three weights: [p](Self::p), [i](Self::i), and [d](Self::d). These can be used to figure out how much each term from [Pid] contributed to the final [output](Self::output) value which should be taken as the final controller output for this iteration:
///
/// ```rust
/// use physical::control::pid::{Pid, ControlOutput};
///
/// // Setup controller
/// let mut pid = Pid::new(15.0, 100.0);
/// pid.p(10.0, 100.0).i(1.0, 100.0).d(2.0, 100.0);
///
/// // Input an example value and get a report for an output iteration
/// let output = pid.next_control_output(26.2456);
/// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output);
/// ```
#[derive(Debug, PartialEq, Eq)]
pub struct ControlOutput<T: Number> {
/// Contribution of the P term to the output.
pub p: T,
/// Contribution of the I term to the output.
///
/// 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,
/// Output of the PID controller.
pub output: T,
}
impl<T> Pid<T> impl<T> Pid<T>
where 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
/// ///
@ -209,8 +173,8 @@ impl<T> Pid<T>
self self
} }
/// Given a new measurement, calculates the next [control output](ControlOutput). /// Given a new measurement, calculates the next control setting.
pub fn next_control_output(&mut self, measurement: T) -> ControlOutput<T> { pub fn next_control_output(&mut self, measurement: T) -> T {
// Calculate the error between the ideal setpoint and the current // Calculate the error between the ideal setpoint and the current
// measurement to compare against // measurement to compare against
let error = self.setpoint - measurement; let error = self.setpoint - measurement;
@ -232,8 +196,8 @@ impl<T> Pid<T>
// 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.as_ref() { 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.d_gain;
self.prev_measurement = Some(measurement); self.prev_measurement = Some(measurement);
@ -244,13 +208,7 @@ impl<T> Pid<T>
let output = p + self.integral_term + d; let output = p + self.integral_term + d;
let output = apply_limit(self.output_limit, output); let output = apply_limit(self.output_limit, output);
// Return the individual term's contributions and the final output output
ControlOutput {
p,
i: self.integral_term,
d,
output,
}
} }
/// Resets the integral term back to zero, this may drastically change the /// Resets the integral term back to zero, this may drastically change the
@ -268,7 +226,6 @@ fn apply_limit<T: Number>(limit: T, value: T) -> T {
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::Pid; use super::Pid;
use super::ControlOutput;
/// Proportional-only controller operation and limits /// Proportional-only controller operation and limits
#[test] #[test]
@ -278,11 +235,11 @@ mod tests {
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).output, 20.0); assert_eq!(pid.next_control_output(0.0), 20.0);
// Test proportional limit // Test proportional limit
pid.p_limit = 10.0; pid.p_limit = 10.0;
assert_eq!(pid.next_control_output(0.0).output, 10.0); assert_eq!(pid.next_control_output(0.0), 10.0);
} }
/// Derivative-only controller operation and limits /// Derivative-only controller operation and limits
@ -292,14 +249,14 @@ mod tests {
pid.p(0.0, 100.0).i(0.0, 100.0).d(2.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 // Test that there's no derivative since it's the first measurement
assert_eq!(pid.next_control_output(0.0).output, 0.0); assert_eq!(pid.next_control_output(0.0), 0.0);
// Test that there's now a derivative // Test that there's now a derivative
assert_eq!(pid.next_control_output(5.0).output, -10.0); assert_eq!(pid.next_control_output(5.0), -10.0);
// Test derivative limit // Test derivative limit
pid.d_limit = 5.0; pid.d_limit = 5.0;
assert_eq!(pid.next_control_output(10.0).output, -5.0); assert_eq!(pid.next_control_output(10.0), -5.0);
} }
/// Integral-only controller operation and limits /// Integral-only controller operation and limits
@ -309,26 +266,26 @@ mod tests {
pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0);
// Test basic integration // Test basic integration
assert_eq!(pid.next_control_output(0.0).output, 20.0); assert_eq!(pid.next_control_output(0.0), 20.0);
assert_eq!(pid.next_control_output(0.0).output, 40.0); assert_eq!(pid.next_control_output(0.0), 40.0);
assert_eq!(pid.next_control_output(5.0).output, 50.0); assert_eq!(pid.next_control_output(5.0), 50.0);
// Test limit // Test limit
pid.i_limit = 50.0; pid.i_limit = 50.0;
assert_eq!(pid.next_control_output(5.0).output, 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).output, 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.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), -20.0);
assert_eq!(pid2.next_control_output(0.0).output, -40.0); assert_eq!(pid2.next_control_output(0.0), -40.0);
pid2.i_limit = 50.0; pid2.i_limit = 50.0;
assert_eq!(pid2.next_control_output(-5.0).output, -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).output, -40.0); assert_eq!(pid2.next_control_output(-15.0), -40.0);
} }
/// Checks that a full PID controller's limits work properly through multiple output iterations /// Checks that a full PID controller's limits work properly through multiple output iterations
@ -338,12 +295,10 @@ mod tests {
pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.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); let out = pid.next_control_output(0.0);
assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out, 1.0);
assert_eq!(out.output, 1.0);
let out = pid.next_control_output(20.0); let out = pid.next_control_output(20.0);
assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out, -1.0);
assert_eq!(out.output, -1.0);
} }
/// Combined PID operation /// Combined PID operation
@ -353,28 +308,16 @@ mod tests {
pid.p(1.0, 100.0).i(0.1, 100.0).d(1.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); let out = pid.next_control_output(0.0);
assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out, 11.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);
let out = pid.next_control_output(5.0); let out = pid.next_control_output(5.0);
assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out, 1.5);
assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0)
assert_eq!(out.d, -5.0); // -(1.0 * 5.0)
assert_eq!(out.output, 1.5);
let out = pid.next_control_output(11.0); let out = pid.next_control_output(11.0);
assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out, -5.6);
assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1)
assert_eq!(out.d, -6.0); // -(1.0 * 6.0)
assert_eq!(out.output, -5.6);
let out = pid.next_control_output(10.0); let out = pid.next_control_output(10.0);
assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out, 2.4);
assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0)
assert_eq!(out.d, 1.0); // -(1.0 * -1.0)
assert_eq!(out.output, 2.4);
} }
// NOTE: use for new test in future: /// Full PID operation with mixed float checking to make sure they're equal // NOTE: use for new test in future: /// Full PID operation with mixed float checking to make sure they're equal
@ -388,10 +331,7 @@ mod tests {
pid_f64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); pid_f64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0);
for _ in 0..5 { for _ in 0..5 {
assert_eq!( assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32);
pid_f32.next_control_output(0.0).output,
pid_f64.next_control_output(0.0).output as f32
);
} }
} }
@ -406,10 +346,7 @@ mod tests {
pid_i32.p(0, 100).i(0, 100).d(0, 100); pid_i32.p(0, 100).i(0, 100).d(0, 100);
for _ in 0..5 { for _ in 0..5 {
assert_eq!( assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32);
pid_i32.next_control_output(0).output,
pid_i8.next_control_output(0i8).output as i32
);
} }
} }
@ -420,22 +357,11 @@ mod tests {
pid.p(1.0, 100.0).i(0.1, 100.0).d(1.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); let out = pid.next_control_output(0.0);
assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out, 11.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); pid.setpoint(0.0);
assert_eq!( assert_eq!(pid.next_control_output(0.0), 1.0);
pid.next_control_output(0.0),
ControlOutput {
p: 0.0,
i: 1.0,
d: -0.0,
output: 1.0
}
);
} }
/// Make sure negative limits don't break the controller /// Make sure negative limits don't break the controller
@ -445,9 +371,6 @@ mod tests {
pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0);
let out = pid.next_control_output(0.0); let out = pid.next_control_output(0.0);
assert_eq!(out.p, 10.0); assert_eq!(out, 10.0);
assert_eq!(out.i, 10.0);
assert_eq!(out.d, 0.0);
assert_eq!(out.output, 10.0);
} }
} }