feat(faderboard): implement pid controller

firmware/faderboard/Cargo.lock

@@ -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"

firmware/faderboard/Cargo.toml

@@ -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"

firmware/faderboard/src/main.rs

@@ -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);
     }
 }

firmware/faderboard/src/pid.rs

@@ -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())
+        }
+    }
+}