feat(hardware): implement rumble (untested)

src/config.rs

@@ -16,6 +16,7 @@ use embassy_rp::{
 use packed_struct::{derive::PackedStruct, PackedStruct};
 
 use crate::{
+    gcc_hid::SIGNAL_CHANGE_RUMBLE_STRENGTH,
     helpers::{PackedFloat, ToPackedFloatArray, ToRegularArray, XyValuePair},
     input::{
         read_ext_adc, Stick, StickAxis, StickState, FLOAT_ORIGIN, SPI_ACS_SHARED, SPI_CCS_SHARED,
@@ -1548,7 +1549,7 @@ async fn configuration_main_loop<
                         })
                     .clamp(0, MAX_RUMBLE_STRENGTH) as u8;
 
-                    // TODO: fire a test rumble here
+                    SIGNAL_CHANGE_RUMBLE_STRENGTH.signal(*to_adjust);
 
                     override_gcc_state_and_wait(&OverrideGcReportInstruction {
                         report: match GcReport::default() {
@@ -1559,6 +1560,7 @@ async fn configuration_main_loop<
                                 a.buttons_2.button_l = true;
                                 a.buttons_1.button_x = true;
                                 a.buttons_1.button_a = true;
+                                a.buttons_2.button_z = true; // makes the controller rumble in smashscope
                                 a.stick_x = 127;
                                 a.stick_y = 127;
                                 a.cstick_x = 127;

src/gcc_hid.rs

@@ -6,18 +6,30 @@ use core::default::Default;
 
 use defmt::{debug, info, trace, warn, Format};
 use embassy_futures::join::join;
-use embassy_rp::{peripherals::USB, usb::Driver};
+use embassy_rp::{
+    peripherals::{PWM_CH4, PWM_CH6, USB},
+    pwm::Pwm,
+    usb::Driver,
+};
 
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
 use embassy_time::{Duration, Instant, Ticker};
 use embassy_usb::{
     class::hid::{HidReaderWriter, ReportId, RequestHandler, State},
     control::OutResponse,
     Builder, Handler,
 };
+use libm::powf;
 use packed_struct::{derive::PackedStruct, PackedStruct};
 
 use crate::input::CHANNEL_GCC_STATE;
 
+static SIGNAL_RUMBLE: Signal<CriticalSectionRawMutex, bool> = Signal::new();
+
+/// We could turn the config change signal into a PubSubChannel instead, but that
+/// would just transmit unnecessary amounts of data.
+pub static SIGNAL_CHANGE_RUMBLE_STRENGTH: Signal<CriticalSectionRawMutex, u8> = Signal::new();
+
 #[rustfmt::skip]
 pub const GCC_REPORT_DESCRIPTOR: &[u8] = &[
     0x05, 0x01,        // Usage Page (Generic Desktop Ctrls)
@@ -385,7 +397,8 @@ pub async fn usb_transfer_task(
             let mut buf = [0u8; 5];
             match reader.read(&mut buf).await {
                 Ok(_e) => {
-                    debug!("READ SOMETHIN: {:08b}", buf)
+                    debug!("READ SOMETHIN: {:08b}", buf);
+                    SIGNAL_RUMBLE.signal((buf[1] & 0x01) != 0);
                 }
                 Err(e) => {
                     warn!("Failed to read: {:?}", e);
@@ -401,3 +414,36 @@ pub async fn usb_transfer_task(
 
     join(usb_fut_wrapped, join(in_fut, out_fut)).await;
 }
+
+fn calc_rumble_power(strength: u8) -> u16 {
+    if strength > 0 {
+        powf(2.0, 7.0 + ((strength as f32 - 3.0) / 8.0)) as u16
+    } else {
+        0
+    }
+}
+
+#[embassy_executor::task]
+pub async fn rumble_task(
+    strength: u8,
+    pwm_rumble: Pwm<'static, PWM_CH4>,
+    pwm_brake: Pwm<'static, PWM_CH6>,
+) {
+    let mut rumble_power = calc_rumble_power(strength);
+
+    loop {
+        let new_rumble_status = SIGNAL_RUMBLE.wait().await;
+
+        if let Some(new_strength) = SIGNAL_CHANGE_RUMBLE_STRENGTH.try_take() {
+            rumble_power = calc_rumble_power(new_strength);
+        }
+
+        if new_rumble_status {
+            pwm_rumble.set_counter(rumble_power);
+            pwm_brake.set_counter(0);
+        } else {
+            pwm_rumble.set_counter(0);
+            pwm_brake.set_counter(255);
+        }
+    }
+}

src/input.rs

@@ -499,14 +499,13 @@ pub async fn update_stick_states_task(
     spi: Spi<'static, SPI0, embassy_rp::spi::Blocking>,
     spi_acs: Output<'static, AnyPin>,
     spi_ccs: Output<'static, AnyPin>,
-    controller_config: ControllerConfig,
+    mut controller_config: ControllerConfig,
 ) {
     Timer::after_secs(1).await;
     *SPI_SHARED.lock().await = Some(spi);
     *SPI_ACS_SHARED.lock().await = Some(spi_acs);
     *SPI_CCS_SHARED.lock().await = Some(spi_ccs);
 
-    let mut controller_config = controller_config;
     let mut controlstick_params = StickParams::from_stick_config(&controller_config.astick_config);
     let mut cstick_params = StickParams::from_stick_config(&controller_config.cstick_config);
     let mut filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);

src/main.rs

@@ -35,6 +35,8 @@ use gpio::{Level, Output};
 use input::{update_button_state_task, update_stick_states_task};
 use static_cell::StaticCell;
 
+use crate::config::enter_config_mode_task;
+use crate::gcc_hid::rumble_task;
 use crate::input::input_integrity_benchmark;
 
 use {defmt_rtt as _, panic_probe as _};
@@ -84,6 +86,20 @@ fn main() -> ! {
     let spi_acs = Output::new(AnyPin::from(p_acs), Level::High); // active low
     let spi_ccs = Output::new(AnyPin::from(p_ccs), Level::High); // active low
 
+    let mut rumble_config: embassy_rp::pwm::Config = Default::default();
+    rumble_config.top = 255;
+    rumble_config.enable = true;
+    rumble_config.compare_b = 0;
+
+    let mut brake_config = rumble_config.clone();
+    brake_config.compare_b = 255;
+
+    let pwm_rumble = Pwm::new_output_b(p.PWM_CH4, p.PIN_25, rumble_config.clone());
+    let pwm_brake = Pwm::new_output_b(p.PWM_CH6, p.PIN_29, brake_config.clone());
+
+    pwm_rumble.set_counter(0);
+    pwm_brake.set_counter(255);
+
     spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
         let executor1 = EXECUTOR1.init(Executor::new());
         debug!("Mana");
@@ -95,6 +111,14 @@ fn main() -> ! {
                     controller_config.input_consistency_mode,
                 ))
                 .unwrap();
+            spawner.spawn(enter_config_mode_task()).unwrap();
+            spawner
+                .spawn(rumble_task(
+                    controller_config.rumble_strength,
+                    pwm_rumble,
+                    pwm_brake,
+                ))
+                .unwrap();
             // spawner.spawn(input_integrity_benchmark()).unwrap();
             spawner
                 .spawn(update_button_state_task(
@@ -130,17 +154,6 @@ fn main() -> ! {
     //     ))
     //     .unwrap();
 
-    let mut pwm_config: embassy_rp::pwm::Config = Default::default();
-    pwm_config.top = 255;
-    pwm_config.enable = true;
-    pwm_config.compare_b = 255;
-
-    // let pwm_rumble = Pwm::new_output_b(p.PWM_CH4, p.PIN_25, pwm_config.clone());
-    // let pwm_brake = Pwm::new_output_b(p.PWM_CH6, p.PIN_29, pwm_config.clone());
-
-    // pwm_rumble.set_counter(0);
-    // pwm_brake.set_counter(255);
-
     let executor0 = EXECUTOR0.init(Executor::new());
     info!("Initialized.");