137 lines
3.3 KiB
Rust
137 lines
3.3 KiB
Rust
use embedded_hal::digital::v2::PinState;
|
|
|
|
pub use fixed::types::I16F16 as FixedI32;
|
|
|
|
pub use fixed_macro::types::I16F16 as FixedI32;
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub struct PidConfig<const FORWARD_HIGH: bool> {
|
|
d_gain: FixedI32,
|
|
i_gain: FixedI32,
|
|
i_max: FixedI32,
|
|
i_min: FixedI32,
|
|
p_gain: FixedI32,
|
|
}
|
|
|
|
impl<const FORWARD_HIGH: bool> PidConfig<FORWARD_HIGH> {
|
|
pub fn new() -> Self {
|
|
Self {
|
|
d_gain: FixedI32::ZERO,
|
|
i_gain: FixedI32::ZERO,
|
|
i_max: FixedI32::ZERO,
|
|
i_min: FixedI32::ZERO,
|
|
p_gain: FixedI32::ZERO,
|
|
}
|
|
}
|
|
|
|
pub fn d_gain(mut self, d_gain: FixedI32) -> Self {
|
|
self.d_gain = d_gain;
|
|
self
|
|
}
|
|
|
|
pub fn i_gain(mut self, i_gain: FixedI32) -> Self {
|
|
self.i_gain = i_gain;
|
|
self
|
|
}
|
|
|
|
pub fn i_max(mut self, i_max: FixedI32) -> Self {
|
|
self.i_max = i_max;
|
|
self
|
|
}
|
|
|
|
pub fn i_min(mut self, i_min: FixedI32) -> Self {
|
|
self.i_min = i_min;
|
|
self
|
|
}
|
|
|
|
pub fn p_gain(mut self, p_gain: FixedI32) -> Self {
|
|
self.p_gain = p_gain;
|
|
self
|
|
}
|
|
}
|
|
|
|
use inner::{Config, Dir};
|
|
|
|
pub struct Pid<'a, CONFIG, const FORWARD_HIGH: bool>
|
|
where
|
|
CONFIG: Config<FORWARD_HIGH> + Dir,
|
|
{
|
|
config: &'a CONFIG,
|
|
d_state: FixedI32,
|
|
i_state: FixedI32,
|
|
}
|
|
|
|
impl<'a, CONFIG, const FORWARD_HIGH: bool> Pid<'a, CONFIG, FORWARD_HIGH>
|
|
where
|
|
CONFIG: Config<FORWARD_HIGH> + Dir,
|
|
{
|
|
pub fn new(config: &'a CONFIG, initial_position: u16) -> Self {
|
|
Self {
|
|
config,
|
|
d_state: FixedI32::saturating_from_num(initial_position),
|
|
i_state: FixedI32::ZERO,
|
|
}
|
|
}
|
|
|
|
pub fn update(&mut self, error: i16, position: u16) -> (u16, PinState) {
|
|
let config = self.config.config();
|
|
let error = FixedI32::from_num(error);
|
|
let position = FixedI32::saturating_from_num(position);
|
|
|
|
let p = config.p_gain * error;
|
|
|
|
self.i_state = match self.i_state + error {
|
|
i if i > config.i_max => config.i_max,
|
|
i if i < config.i_min => config.i_min,
|
|
i => i,
|
|
};
|
|
|
|
let i = config.i_gain * self.i_state;
|
|
|
|
let d = config.d_gain * (self.d_state - position);
|
|
self.d_state = position;
|
|
|
|
let output = (p + i + d).round();
|
|
return (
|
|
output.abs().saturating_to_num::<u16>(),
|
|
self.config.dir(output),
|
|
);
|
|
}
|
|
}
|
|
|
|
mod inner {
|
|
use embedded_hal::digital::v2::PinState;
|
|
|
|
use fixed::types::I16F16;
|
|
|
|
use super::PidConfig;
|
|
|
|
pub trait Config<const FORWARD_HIGH: bool> {
|
|
fn config(&self) -> &PidConfig<FORWARD_HIGH>;
|
|
}
|
|
|
|
impl<const FORWARD_HIGH: bool> Config<FORWARD_HIGH> for PidConfig<FORWARD_HIGH> {
|
|
fn config(&self) -> &PidConfig<FORWARD_HIGH> {
|
|
self
|
|
}
|
|
}
|
|
|
|
pub trait Dir {
|
|
fn dir(&self, output: I16F16) -> PinState;
|
|
}
|
|
|
|
impl Dir for PidConfig<false> {
|
|
fn dir(&self, output: I16F16) -> PinState {
|
|
//! If FORWARD_HIGH is false, return PinState::High for reverse direction
|
|
PinState::from(output.is_negative())
|
|
}
|
|
}
|
|
|
|
impl Dir for PidConfig<true> {
|
|
fn dir(&self, output: I16F16) -> PinState {
|
|
//! If FORWARD_HIGH is true, return PinState::High for forward direction
|
|
PinState::from(output.is_positive())
|
|
}
|
|
}
|
|
}
|