diff --git a/firmware/faderboard/Cargo.lock b/firmware/faderboard/Cargo.lock index a9994d5..5b3adf3 100644 --- a/firmware/faderboard/Cargo.lock +++ b/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" diff --git a/firmware/faderboard/Cargo.toml b/firmware/faderboard/Cargo.toml index 5c071fa..e67ee54 100644 --- a/firmware/faderboard/Cargo.toml +++ b/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" diff --git a/firmware/faderboard/src/main.rs b/firmware/faderboard/src/main.rs index 1becd8a..5b37df6 100644 --- a/firmware/faderboard/src/main.rs +++ b/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::::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); } } diff --git a/firmware/faderboard/src/pid.rs b/firmware/faderboard/src/pid.rs new file mode 100644 index 0000000..045a6d4 --- /dev/null +++ b/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 { + d_gain: FixedI32, + i_gain: FixedI32, + i_max: FixedI32, + i_min: FixedI32, + p_gain: FixedI32, +} + +impl PidConfig { + 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 + 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 + 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::(), + self.config.dir(output), + ); + } +} + +mod inner { + use embedded_hal::digital::v2::PinState; + + use fixed::types::I16F16; + + use super::PidConfig; + + pub trait Config { + fn config(&self) -> &PidConfig; + } + + impl Config for PidConfig { + fn config(&self) -> &PidConfig { + self + } + } + + pub trait Dir { + fn dir(&self, output: I16F16) -> PinState; + } + + impl Dir for PidConfig { + 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 { + fn dir(&self, output: I16F16) -> PinState { + //! If FORWARD_HIGH is true, return PinState::High for forward direction + PinState::from(output.is_positive()) + } + } +}