feat(faderboard): implement pid controller
This commit is contained in:
parent
8c3625c8da
commit
d8527ce5e7
|
|
@ -2,6 +2,12 @@
|
|||
# It is not intended for manual editing.
|
||||
version = 3
|
||||
|
||||
[[package]]
|
||||
name = "az"
|
||||
version = "1.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973"
|
||||
|
||||
[[package]]
|
||||
name = "bare-metal"
|
||||
version = "0.2.5"
|
||||
|
|
@ -40,6 +46,12 @@ dependencies = [
|
|||
"vcell",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "bytemuck"
|
||||
version = "1.16.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b236fc92302c97ed75b38da1f4917b5cdda4984745740f153a5d3059e48d725e"
|
||||
|
||||
[[package]]
|
||||
name = "cast"
|
||||
version = "0.2.7"
|
||||
|
|
@ -49,6 +61,12 @@ dependencies = [
|
|||
"rustc_version 0.4.0",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "cfg-if"
|
||||
version = "1.0.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
|
||||
|
||||
[[package]]
|
||||
name = "cortex-m"
|
||||
version = "0.7.7"
|
||||
|
|
@ -81,6 +99,12 @@ dependencies = [
|
|||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "crunchy"
|
||||
version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7"
|
||||
|
||||
[[package]]
|
||||
name = "embedded-hal"
|
||||
version = "0.2.7"
|
||||
|
|
@ -97,12 +121,72 @@ version = "0.1.0"
|
|||
dependencies = [
|
||||
"cortex-m",
|
||||
"cortex-m-rt",
|
||||
"embedded-hal",
|
||||
"fixed",
|
||||
"fixed-macro",
|
||||
"panic-halt",
|
||||
"stm32f0xx-hal",
|
||||
"usb-device",
|
||||
"usbd-serial",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "fixed"
|
||||
version = "1.27.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2fc715d38bea7b5bf487fcd79bcf8c209f0b58014f3018a7a19c2b855f472048"
|
||||
dependencies = [
|
||||
"az",
|
||||
"bytemuck",
|
||||
"half",
|
||||
"typenum",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "fixed-macro"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1fd0c48af8cb14e02868f449f8a2187bd78af7a08da201fdc78d518ecb1675bc"
|
||||
dependencies = [
|
||||
"fixed",
|
||||
"fixed-macro-impl",
|
||||
"fixed-macro-types",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "fixed-macro-impl"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c93086f471c0a1b9c5e300ea92f5cd990ac6d3f8edf27616ef624b8fa6402d4b"
|
||||
dependencies = [
|
||||
"fixed",
|
||||
"paste",
|
||||
"proc-macro-error",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "fixed-macro-types"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "044a61b034a2264a7f65aa0c3cd112a01b4d4ee58baace51fead3f21b993c7e4"
|
||||
dependencies = [
|
||||
"fixed",
|
||||
"fixed-macro-impl",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "half"
|
||||
version = "2.4.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6dd08c532ae367adf81c312a4580bc67f1d0fe8bc9c460520283f4c0ff277888"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"crunchy",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nb"
|
||||
version = "0.1.3"
|
||||
|
|
@ -124,6 +208,36 @@ version = "0.2.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812"
|
||||
|
||||
[[package]]
|
||||
name = "paste"
|
||||
version = "1.0.15"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro-error"
|
||||
version = "1.0.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c"
|
||||
dependencies = [
|
||||
"proc-macro-error-attr",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"version_check",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro-error-attr"
|
||||
version = "1.0.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"version_check",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro2"
|
||||
version = "1.0.86"
|
||||
|
|
@ -231,6 +345,12 @@ dependencies = [
|
|||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "typenum"
|
||||
version = "1.17.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825"
|
||||
|
||||
[[package]]
|
||||
name = "unicode-ident"
|
||||
version = "1.0.12"
|
||||
|
|
@ -260,6 +380,12 @@ version = "0.1.3"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002"
|
||||
|
||||
[[package]]
|
||||
name = "version_check"
|
||||
version = "0.9.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f"
|
||||
|
||||
[[package]]
|
||||
name = "void"
|
||||
version = "1.0.2"
|
||||
|
|
|
|||
|
|
@ -8,6 +8,9 @@ edition = "2021"
|
|||
[dependencies]
|
||||
cortex-m = "0.7.7"
|
||||
cortex-m-rt = "0.7.3"
|
||||
embedded-hal = "0.2.7"
|
||||
fixed = "1.27.0"
|
||||
fixed-macro = "1.2.0"
|
||||
panic-halt = "0.2.0"
|
||||
stm32f0xx-hal = { git = "https://github.com/lujoga/stm32f0xx-hal.git", branch = "v0.18.0-backports", features = ["rt", "stm32f072", "stm32-usbd"] }
|
||||
usb-device = "0.2.9"
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#![no_std]
|
||||
#![no_main]
|
||||
|
||||
mod pid;
|
||||
mod touch;
|
||||
mod usb;
|
||||
|
||||
|
|
@ -8,6 +9,8 @@ use cortex_m_rt::entry;
|
|||
|
||||
use panic_halt as _;
|
||||
|
||||
use pid::{FixedI32, Pid, PidConfig};
|
||||
|
||||
use stm32f0xx_hal as hal;
|
||||
|
||||
use hal::adc::Adc;
|
||||
|
|
@ -140,7 +143,17 @@ fn main() -> ! {
|
|||
let touch = Touch::new(sys_cfg, p.EXTI, delay.clone());
|
||||
touch.enable();
|
||||
|
||||
let pid_config = PidConfig::<false>::new()
|
||||
.d_gain(FixedI32!(0))
|
||||
.i_gain(FixedI32!(0))
|
||||
.i_max(FixedI32!(0))
|
||||
.i_min(FixedI32!(0))
|
||||
.p_gain(FixedI32!(0.5));
|
||||
let mut pid = Pid::new(&pid_config, 2048);
|
||||
|
||||
loop {
|
||||
usb.poll();
|
||||
|
||||
let (_duty, _dir) = pid.update(0, 2048);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,136 @@
|
|||
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())
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue