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

pid (#12)

Browse Source 
Added pid control

Co-authored-by: Zachary Sunforge <zachary.sunforge@bfpower.io>
Reviewed-on: #12
This commit is contained in:
Zachary Levy
2024-06-07 23:41:54 +00:00
parent 38044cb945
commit 3f79ef86d8
4 changed files with 381 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
Cargo.toml
View File

@ -24,12 +24,15 @@ readme = "README.md"
license = "MIT"
#----- no-std ----------------------------------
# Math
# Numbers
[workspace.dependencies.num-traits]
version = "0.2.*"
default-features = false
[workspace.dependencies.libm]
version = "0.2.*"
# Units of measurement
[workspace.dependencies.uom]
version = "0.35.*"
version = "0.36.*"
default-features = false
features = ["f32", "si"]
# Logging
@ -39,10 +42,6 @@ version = "0.1.*"
version = "0.3.*"
[workspace.dependencies.defmt-rtt]
version = "0.4.*"
# Serialization
[workspace.dependencies.parity-scale-codec]
version = "3.6.*"
default-features = false
# Embedded-HAL
[workspace.dependencies.embedded-hal]
version = "1.0.*"
@ -50,9 +49,12 @@ version = "1.0.*"
version = "1.0.*"
# Memory
[workspace.dependencies.static_cell]
version = "2.0.*"
[workspace.dependencies.heapless]
version = "0.8.*"
version = "2.1.*"
# Serioalization
[workspace.dependencies.serde]
version = "1.0.*"
default-features = false
features = ["derive"]
# Other embedded utilities
[workspace.dependencies.cortex-m]
version = "0.7.*"
@ -75,7 +77,7 @@ version = "0.1.*"
version = "0.3.*"
features = ["defmt", "defmt-timestamp-uptime"]
[workspace.dependencies.embassy-sync]
version = "0.5.*"
version = "0.6.*"
features = ["defmt"]
[workspace.dependencies.embassy-embedded-hal]
version = "0.1.*"
@ -112,11 +114,13 @@ license.workspace = true
[features]
thermocouple_k = []
lm35 = []
pid = []
[dependencies]
uom = { workspace = true }
parity-scale-codec = { workspace = true }
num-traits = { workspace = true }
libm = { workspace = true }
serde = { workspace = true, optional = true }
#---------------------------------------------------------------------------------------------------------------------
#----- Profiles ------------------------

2
src/control/mod.rs Normal file
View File

@ -0,0 +1,2 @@
#[cfg(feature = "pid")]
pub mod pid;

363
src/control/pid.rs Normal file
View File

@ -0,0 +1,363 @@
//! 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 physical::control::pid::Pid;
//!
//! // Create a new proportional-only PID controller with a setpoint of 15
//! let mut pid = Pid::new(15.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
//! let output = pid.next_control_output(10.0);
//!
//! // Show that the error is correct by multiplying by our kp
//! 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, 50.0); // <--
//!
//! // Add a new integral term to the controller and input again
//! pid.integral_gain = 1.0;
//! pid.integral_limit = 100.0;
//! let output = pid.next_control_output(10.0);
//!
//! // Now that the integral makes the controller stateful, it will change
//! assert_eq!(output, 55.0); // <--
//!
//! // Add our final derivative term and match our setpoint target
//! pid.derivative_gain = 2.0;
//! pid.derivative_limit = 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, -5.0); // <--//!
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};
/// A trait for any numeric type usable in the PID controller
///
/// This trait is automatically implemented for all types that satisfy `PartialOrd + num_traits::Signed + Copy`. This includes all of the signed float types and builtin integer except for [isize]:
/// - [i8]
/// - [i16]
/// - [i32]
/// - [i64]
/// - [i128]
/// - [f32]
/// - [f64]
///
/// As well as any user type that matches the requirements
pub trait Number: PartialOrd + num_traits::Signed + Copy {}
// Implement `Number` for all types that
// satisfy `PartialOrd + num_traits::Signed + Copy`.
impl<T: PartialOrd + num_traits::Signed + Copy> Number for T {}
/// Adjustable proportional-integral-derivative (PID) controller.
///
/// 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 the gain and limit fields can be safely modified during use.
///
/// # 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.
#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)]
#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))]
pub struct Pid<T: Number> {
/// Ideal setpoint to strive for.
pub setpoint: T,
/// Defines the overall output filter limit.
pub output_limit: T,
/// Proportional gain. The proportional component is dependant only on the error.
/// It is the error * this value.
pub proportional_gain: T,
/// 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 integral_gain: T,
/// 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 derivative_gain: T,
/// Limiter for the proportional term: `-p_limit <= P <= p_limit`.
pub proportional_limit: T,
/// Limiter for the integral term: `-i_limit <= I <= i_limit`.
pub integral_limit: T,
/// Limiter for the derivative term: `-d_limit <= D <= d_limit`.
pub derivative_limit: T,
/// Last calculated integral value if [Pid::i_gain] is used.
integral_term: T,
/// Previously found measurement whilst using the [Pid::next_control_output] method.
prev_measurement: Option<T>,
}
impl<T> Pid<T>
where
T: Number,
{
/// Creates a new controller with the target setpoint and the output limit
pub fn new(setpoint: T, output_limit: T) -> Self {
Self {
setpoint,
output_limit,
proportional_gain: T::zero(),
integral_gain: T::zero(),
derivative_gain: T::zero(),
proportional_limit: T::zero(),
integral_limit: T::zero(),
derivative_limit: T::zero(),
integral_term: T::zero(),
prev_measurement: None,
}
}
/// Sets the [Pid::setpoint] to target for this controller.
pub fn setpoint(&mut self, setpoint: T) -> &mut Self {
self.setpoint = setpoint;
self
}
/// 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;
// Calculate the proportional term and limit to it's individual limit
let p_unbounded = error * self.proportional_gain;
let p = apply_limit(self.proportional_limit, p_unbounded);
// Mitigate output jumps when ki(t) != ki(t-1).
// 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,
// we store the entire term so that we don't need to remember previous
// ki values.
self.integral_term = self.integral_term + error * self.integral_gain;
// Mitigate integral windup: Don't want to keep building up error
// beyond what i_limit will allow.
self.integral_term = apply_limit(self.integral_limit, self.integral_term);
// Mitigate derivative kick: Use the derivative of the measurement
// rather than the derivative of the error.
let d_unbounded = -match self.prev_measurement {
Some(prev_measurement) => measurement - prev_measurement,
None => T::zero(),
} * self.derivative_gain;
self.prev_measurement = Some(measurement);
let d = apply_limit(self.derivative_limit, d_unbounded);
// Calculate the final output by adding together the PID terms, then
// apply the final defined output limit
let output = p + self.integral_term + d;
let output = apply_limit(self.output_limit, 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` (`-abs(limit) <= output <= abs(limit)`).
fn apply_limit<T: Number>(limit: T, value: T) -> T {
num_traits::clamp(value, -limit.abs(), limit.abs())
}
#[cfg(test)]
mod tests {
use super::Pid;
/// Proportional-only controller operation and limits
#[test]
fn proportional() {
let mut pid = Pid::new(10.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);
// Test simple proportional
assert_eq!(pid.next_control_output(0.0), 20.0);
// Test proportional limit
pid.proportional_limit = 10.0;
assert_eq!(pid.next_control_output(0.0), 10.0);
}
/// Derivative-only controller operation and limits
#[test]
fn derivative() {
let mut pid = Pid::new(10.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
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), -10.0);
// Test derivative limit
pid.derivative_limit = 5.0;
assert_eq!(pid.next_control_output(10.0), -5.0);
}
/// Integral-only controller operation and limits
#[test]
fn integral() {
let mut pid = Pid::new(10.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
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.integral_limit = 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), 40.0);
// Test that error integral accumulates negative values
let mut pid2 = Pid::new(-10.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), -40.0);
pid2.integral_limit = 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), -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(10.0, 1.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);
assert_eq!(out, 1.0);
let out = pid.next_control_output(20.0);
assert_eq!(out, -1.0);
}
/// Combined PID operation
#[test]
fn pid() {
let mut pid = Pid::new(10.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);
assert_eq!(out, 11.0);
let out = pid.next_control_output(5.0);
assert_eq!(out, 1.5);
let out = pid.next_control_output(11.0);
assert_eq!(out, -5.6);
let out = pid.next_control_output(10.0);
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
/// PID operation with zero'd values, checking to see if different floats equal each other
#[test]
fn floats_zeros() {
let mut pid_f32 = Pid::new(10.0f32, 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);
pid_f64.proportional_limit = 100.0;
pid_f64.integral_limit = 100.0;
pid_f64.derivative_limit = 100.0;
for _ in 0..5 {
assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32);
}
}
// NOTE: use for new test in future: /// Full PID operation with mixed signed integer checking to make sure they're equal
/// PID operation with zero'd values, checking to see if different floats equal each other
#[test]
fn signed_integers_zeros() {
let mut pid_i8 = Pid::new(10i8, 100);
pid_i8.proportional_limit = 100;
pid_i8.integral_limit = 100;
pid_i8.derivative_limit = 100;
let mut pid_i32 = Pid::new(10i32, 100);
pid_i32.proportional_limit = 100;
pid_i32.integral_limit = 100;
pid_i32.derivative_limit = 100;
for _ in 0..5 {
assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32);
}
}
/// 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.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);
assert_eq!(out, 11.0);
pid.setpoint(0.0);
assert_eq!(pid.next_control_output(0.0), 1.0);
}
/// Make sure negative limits don't break the controller
#[test]
fn negative_limits() {
let mut pid = Pid::new(10.0f32, -10.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);
assert_eq!(out, 10.0);
}
}

1
src/lib.rs
View File

@ -1,6 +1,7 @@
#![no_std]
pub mod transducer;
pub mod control;
pub mod cell;
mod error;