#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum DiscreteIntegrationMethod {
ForwardEuler,
BackwardEuler,
Trapezoidal,
}
impl DiscreteIntegrationMethod {
pub fn from_name(name: &str) -> Option<Self> {
use DiscreteIntegrationMethod::*;
Some(match name {
"forward" => ForwardEuler,
"backward" => BackwardEuler,
"trapezoidal" => Trapezoidal,
_ => return None,
})
}
pub fn name(self) -> &'static str {
use DiscreteIntegrationMethod::*;
match self {
ForwardEuler => "forward",
BackwardEuler => "backward",
Trapezoidal => "trapezoidal",
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct DiscretePid {
pub kp: f64,
pub ki: f64,
pub kd: f64,
pub n: f64,
pub period: f64,
pub method: DiscreteIntegrationMethod,
}
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct DiscretePidState {
pub int_state: f64,
pub int_prev_e: f64,
pub filt_state: f64,
pub filt_prev_v: f64,
}
impl DiscretePid {
pub fn step(&self, state: &mut DiscretePidState, e: f64) -> f64 {
let int_output = match self.method {
DiscreteIntegrationMethod::ForwardEuler => {
let y = state.int_state + self.period * state.int_prev_e;
state.int_state = y;
state.int_prev_e = e;
y
}
DiscreteIntegrationMethod::BackwardEuler => {
let y = state.int_state + self.period * e;
state.int_state = y;
y
}
DiscreteIntegrationMethod::Trapezoidal => {
let y = state.int_state + (self.period / 2.0) * (state.int_prev_e + e);
state.int_state = y;
state.int_prev_e = e;
y
}
};
let v = self.n * (self.kd * e - state.filt_state);
let filt_next = state.filt_state + self.period * state.filt_prev_v;
state.filt_prev_v = v;
state.filt_state = filt_next;
self.kp * e + self.ki * int_output + v
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn pure_p_is_stateless_and_matches_kp_times_error_every_step() {
let pid = DiscretePid {
kp: 3.0,
ki: 0.0,
kd: 0.0,
n: 1.0,
period: 0.01,
method: DiscreteIntegrationMethod::ForwardEuler,
};
let mut state = DiscretePidState::default();
assert_eq!(pid.step(&mut state, 2.0), 6.0);
assert_eq!(pid.step(&mut state, -1.0), -3.0); }
#[test]
fn forward_euler_integral_matches_the_hand_derived_no_feedthrough_recursion() {
let pid = DiscretePid {
kp: 0.0,
ki: 5.0,
kd: 0.0,
n: 1.0,
period: 0.1,
method: DiscreteIntegrationMethod::ForwardEuler,
};
let mut state = DiscretePidState::default();
assert_eq!(pid.step(&mut state, 2.0), 0.0);
assert_eq!(pid.step(&mut state, 2.0), 1.0);
assert_eq!(pid.step(&mut state, 2.0), 2.0);
}
#[test]
fn backward_euler_integral_has_feedthrough_unlike_forward_euler() {
let pid = DiscretePid {
kp: 0.0,
ki: 5.0,
kd: 0.0,
n: 1.0,
period: 0.1,
method: DiscreteIntegrationMethod::BackwardEuler,
};
let mut state = DiscretePidState::default();
assert_eq!(pid.step(&mut state, 2.0), 1.0);
assert_eq!(pid.step(&mut state, 2.0), 2.0);
}
#[test]
fn trapezoidal_integral_averages_current_and_previous_error() {
let pid = DiscretePid {
kp: 0.0,
ki: 1.0,
kd: 0.0,
n: 1.0,
period: 1.0,
method: DiscreteIntegrationMethod::Trapezoidal,
};
let mut state = DiscretePidState::default();
assert_eq!(pid.step(&mut state, 2.0), 1.0);
assert_eq!(pid.step(&mut state, 4.0), 4.0);
}
#[test]
fn pure_d_settles_to_zero_for_a_constant_error_a_real_derivative_property() {
let pid = DiscretePid {
kp: 0.0,
ki: 0.0,
kd: 2.0,
n: 50.0, period: 0.001,
method: DiscreteIntegrationMethod::ForwardEuler,
};
let mut state = DiscretePidState::default();
let mut last = f64::MAX;
for _ in 0..2000 {
last = pid.step(&mut state, 3.0); }
assert!(
last.abs() < 1e-6,
"expected settled derivative ~0, got {last}"
);
}
#[test]
fn from_name_matches_conventional_names_and_rejects_unknown() {
assert_eq!(
DiscreteIntegrationMethod::from_name("backward"),
Some(DiscreteIntegrationMethod::BackwardEuler)
);
assert_eq!(DiscreteIntegrationMethod::from_name("nope"), None);
}
}