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

pid #12

Merged
zack merged 6 commits from pid into master 2024-06-07 23:41:54 +00:00
Conversation 0 Commits 6 Files Changed 4 +394 -11
4 changed files with 394 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 46abb2150c - Show all commits

157
src/control/pid.rs
View File

@ -15,33 +15,25 @@
//! 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);
//! assert_eq!(output, 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);
//! assert_eq!(output, 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);
//! assert_eq!(output, 55.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);
//!
//! assert_eq!(output, -5.0); // <--//!
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
@ -108,11 +100,14 @@ pub struct Pid<T: Number> {
pub setpoint: T,
/// Defines the overall output filter limit.
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,
/// 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,
/// 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,
/// Limiter for the proportional term: `-p_limit <= P <= p_limit`.
pub p_limit: T,
@ -126,37 +121,6 @@ pub struct Pid<T: Number> {
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>
where
T: Number,
@ -209,8 +173,8 @@ impl<T> Pid<T>
self
}
/// Given a new measurement, calculates the next [control output](ControlOutput).
pub fn next_control_output(&mut self, measurement: T) -> ControlOutput<T> {
/// Given a new measurement, calculates the next control setting.
pub fn next_control_output(&mut self, measurement: T) -> T {
// Calculate the error between the ideal setpoint and the current
// measurement to compare against
let error = self.setpoint - measurement;
@ -232,8 +196,8 @@ impl<T> Pid<T>
// Mitigate derivative kick: Use the derivative of the measurement
// rather than the derivative of the error.
let d_unbounded = -match self.prev_measurement.as_ref() {
Some(prev_measurement) => measurement - *prev_measurement,
let d_unbounded = -match self.prev_measurement {
Some(prev_measurement) => measurement - prev_measurement,
None => T::zero(),
} * self.d_gain;
self.prev_measurement = Some(measurement);
@ -244,13 +208,7 @@ impl<T> Pid<T>
let output = p + self.integral_term + d;
let output = apply_limit(self.output_limit, output);
// Return the individual term's contributions and the final output
ControlOutput {
p,
i: self.integral_term,
d,
output,
}
output
}
/// 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)]
mod tests {
use super::Pid;
use super::ControlOutput;
/// Proportional-only controller operation and limits
#[test]
@ -278,11 +235,11 @@ mod tests {
assert_eq!(pid.setpoint, 10.0);
// 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
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
@ -292,14 +249,14 @@ mod tests {
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);
assert_eq!(pid.next_control_output(0.0), 0.0);
// 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
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
@ -309,26 +266,26 @@ mod tests {
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);
assert_eq!(pid.next_control_output(0.0).output, 40.0);
assert_eq!(pid.next_control_output(5.0).output, 50.0);
assert_eq!(pid.next_control_output(0.0), 20.0);
assert_eq!(pid.next_control_output(0.0), 40.0);
assert_eq!(pid.next_control_output(5.0), 50.0);
// Test limit
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
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
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);
assert_eq!(pid2.next_control_output(0.0), -20.0);
assert_eq!(pid2.next_control_output(0.0), -40.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
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
@ -338,12 +295,10 @@ mod tests {
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
assert_eq!(out.output, 1.0);
assert_eq!(out, 1.0);
let out = pid.next_control_output(20.0);
assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0)
assert_eq!(out.output, -1.0);
assert_eq!(out, -1.0);
}
/// 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);
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);
assert_eq!(out, 11.0);
let out = pid.next_control_output(5.0);
assert_eq!(out.p, 5.0); // 1.0 * 5.0
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);
assert_eq!(out, 1.5);
let out = pid.next_control_output(11.0);
assert_eq!(out.p, -1.0); // 1.0 * -1.0
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);
assert_eq!(out, -5.6);
let out = pid.next_control_output(10.0);
assert_eq!(out.p, 0.0); // 1.0 * 0.0
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);
assert_eq!(out, 2.4);
}
// 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);
for _ in 0..5 {
assert_eq!(
pid_f32.next_control_output(0.0).output,
pid_f64.next_control_output(0.0).output as f32
);
assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32);
}
}
@ -406,10 +346,7 @@ mod tests {
pid_i32.p(0, 100).i(0, 100).d(0, 100);
for _ in 0..5 {
assert_eq!(
pid_i32.next_control_output(0).output,
pid_i8.next_control_output(0i8).output as i32
);
assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32);
}
}
@ -420,22 +357,11 @@ mod tests {
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);
assert_eq!(out, 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
}
);
assert_eq!(pid.next_control_output(0.0), 1.0);
}
/// 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);
let out = pid.next_control_output(0.0);
assert_eq!(out.p, 10.0);
assert_eq!(out.i, 10.0);
assert_eq!(out.d, 0.0);
assert_eq!(out.output, 10.0);
assert_eq!(out, 10.0);
}
}