Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove num-traits as a dependency and just use core traits #23

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+87 −85
Open

Remove num-traits as a dependency and just use core traits #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3ae1fe5
Added Functionality to manually set the integral term. This might be …
padalev Oct 26, 2023
6e6b07e
Added dt handover
padalev Oct 26, 2023
dadc377
remove num-traits and use core traits
ripytide Feb 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 6 additions & 11 deletions Cargo.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@ name = "pid"
version = "4.0.0"
edition = "2018"
authors = [
"Ken Elkabany <[email protected]>",
"Owez Griffiths <[email protected]>",
"Ken Elkabany <[email protected]>",
"Owez Griffiths <[email protected]>",
]
license = "MIT OR Apache-2.0"
description = "A PID controller."
Expand All @@ -13,15 +13,10 @@ keywords = ["pid"]
categories = ["no-std", "embedded", "algorithms"]
readme = "README.md"

[dependencies.num-traits]
version = "0.2"
default-features = false

[dependencies.serde]
version = "1.0"
optional = true
default-features = false
features = ["derive"]
[dependencies]
serde = { version = "1.0", default-features = false, features = [
"derive",
], optional = true }

[badges]
travis-ci = { repository = "braincore/pid-rs" }
155 changes: 81 additions & 74 deletions src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,26 +44,23 @@
//! ```
#![no_std]

#[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 {}
trait PartialEqClamp {
fn clamp(self, min: Self, max: Self) -> Self;
}
impl<T> PartialEqClamp for T
where
T: core::cmp::PartialOrd,
{
fn clamp(self, min: Self, max: Self) -> Self {
if self < min {
min
} else if self > max {
max
} else {
self
}
}
}

/// Adjustable proportional-integral-derivative (PID) controller.
///
Expand Down Expand Up @@ -103,8 +100,8 @@ impl<T: PartialOrd + num_traits::Signed + Copy> Number for T {}
///
/// [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> {
#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))]
pub struct Pid<T> {
/// Ideal setpoint to strive for.
pub setpoint: T,
/// Defines the overall output filter limit.
Expand Down Expand Up @@ -145,7 +142,7 @@ pub struct Pid<T: Number> {
/// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output);
/// ```
#[derive(Debug, PartialEq, Eq)]
pub struct ControlOutput<T: Number> {
pub struct ControlOutput<T> {
/// Contribution of the P term to the output.
pub p: T,
/// Contribution of the I term to the output.
Expand All @@ -160,7 +157,14 @@ pub struct ControlOutput<T: Number> {

impl<T> Pid<T>
where
T: Number,
T: core::ops::Sub<T, Output = T>
+ core::ops::Add<T, Output = T>
+ core::ops::Mul<T, Output = T>
+ core::ops::Div<T, Output = T>
+ core::cmp::PartialOrd
+ Default
+ core::ops::Neg<Output = T>
+ core::marker::Copy,
{
/// Creates a new controller with the target setpoint and the output limit
///
Expand All @@ -172,13 +176,13 @@ where
Self {
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(),
kp: T::default(),
ki: T::default(),
kd: T::default(),
p_limit: T::default(),
i_limit: T::default(),
d_limit: T::default(),
integral_term: T::default(),
prev_measurement: None,
}
}
Expand Down Expand Up @@ -210,64 +214,67 @@ where
self
}

/// Given a new measurement, calculates the next [control output](ControlOutput).
/// Given a new measurement and dt, calculates the next [control output](ControlOutput).
///
/// # Panics
///
/// - If a setpoint has not been set via `update_setpoint()`.
pub fn next_control_output(&mut self, measurement: T) -> ControlOutput<T> {
pub fn next_control_output(&mut self, measurement: T, dt: T) -> ControlOutput<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.kp;
let p = apply_limit(self.p_limit, p_unbounded);
let p = p_unbounded.clamp(-self.p_limit, self.p_limit);

// 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.ki;
self.integral_term = self.integral_term + error * self.ki * dt;

// Mitigate integral windup: Don't want to keep building up error
// beyond what i_limit will allow.
self.integral_term = apply_limit(self.i_limit, self.integral_term);
self.integral_term = self.integral_term.clamp(-self.i_limit, self.i_limit);

// 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,
None => T::zero(),
} * self.kd;
None => T::default(),
} * self.kd
/ dt;
self.prev_measurement = Some(measurement);
let d = apply_limit(self.d_limit, d_unbounded);
let d = d_unbounded.clamp(-self.d_limit, self.d_limit);

// 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);
let output_unbounded = p + self.integral_term + d;
let output = output_unbounded.clamp(-self.output_limit, self.output_limit);

// Return the individual term's contributions and the final output
ControlOutput {
p,
i: self.integral_term,
d,
output: 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();
self.set_integral_term(T::default());
}
}

/// 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())
/// Set integral term to custom value. This might be important to set pid
/// controller to previous state after an interruption or crash
pub fn set_integral_term(&mut self, integral_term: impl Into<T>) -> &mut Self {
self.integral_term = integral_term.into();
self
}
}

#[cfg(test)]
Expand All @@ -283,11 +290,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, 1.0).output, 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, 1.0).output, 10.0);
}

/// Derivative-only controller operation and limits
Expand All @@ -297,14 +304,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, 1.0).output, 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, 1.0).output, -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, 1.0).output, -5.0);
}

/// Integral-only controller operation and limits
Expand All @@ -314,26 +321,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, 1.0).output, 20.0);
assert_eq!(pid.next_control_output(0.0, 1.0).output, 40.0);
assert_eq!(pid.next_control_output(5.0, 1.0).output, 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, 1.0).output, 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, 1.0).output, 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, 1.0).output, -20.0);
assert_eq!(pid2.next_control_output(0.0, 1.0).output, -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, 1.0).output, -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, 1.0).output, -40.0);
}

/// Checks that a full PID controller's limits work properly through multiple output iterations
Expand All @@ -342,11 +349,11 @@ mod tests {
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);
let out = pid.next_control_output(0.0, 1.0);
assert_eq!(out.p, 10.0); // 1.0 * 10.0
assert_eq!(out.output, 1.0);

let out = pid.next_control_output(20.0);
let out = pid.next_control_output(20.0, 1.0);
assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0)
assert_eq!(out.output, -1.0);
}
Expand All @@ -357,25 +364,25 @@ mod tests {
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);
let out = pid.next_control_output(0.0, 1.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);

let out = pid.next_control_output(5.0);
let out = pid.next_control_output(5.0, 1.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);

let out = pid.next_control_output(11.0);
let out = pid.next_control_output(11.0, 1.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);

let out = pid.next_control_output(10.0);
let out = pid.next_control_output(10.0, 1.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)
Expand All @@ -394,8 +401,8 @@ mod tests {

for _ in 0..5 {
assert_eq!(
pid_f32.next_control_output(0.0).output,
pid_f64.next_control_output(0.0).output as f32
pid_f32.next_control_output(0.0, 1.0).output,
pid_f64.next_control_output(0.0, 1.0).output as f32
);
}
}
Expand All @@ -412,8 +419,8 @@ mod tests {

for _ in 0..5 {
assert_eq!(
pid_i32.next_control_output(0).output,
pid_i8.next_control_output(0i8).output as i32
pid_i32.next_control_output(0, 1).output,
pid_i8.next_control_output(0i8, 1i8).output as i32
);
}
}
Expand All @@ -424,7 +431,7 @@ mod tests {
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);
let out = pid.next_control_output(0.0, 1.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)
Expand All @@ -433,7 +440,7 @@ mod tests {
pid.setpoint(0.0);

assert_eq!(
pid.next_control_output(0.0),
pid.next_control_output(0.0, 1.0),
ControlOutput {
p: 0.0,
i: 1.0,
Expand All @@ -449,7 +456,7 @@ mod tests {
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);

let out = pid.next_control_output(0.0);
let out = pid.next_control_output(0.0, 1.0);
assert_eq!(out.p, 10.0);
assert_eq!(out.i, 10.0);
assert_eq!(out.d, 0.0);
Expand Down