diff --git a/src/config.rs b/src/config.rs index 2b98575..53d21f8 100644 --- a/src/config.rs +++ b/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; diff --git a/src/gcc_hid.rs b/src/gcc_hid.rs index f3963f0..63e4528 100644 --- a/src/gcc_hid.rs +++ b/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 = 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 = 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); + } + } +} diff --git a/src/input.rs b/src/input.rs index c69b0a5..1bcd791 100644 --- a/src/input.rs +++ b/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); diff --git a/src/main.rs b/src/main.rs index 939a359..1dc9b72 100644 --- a/src/main.rs +++ b/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.");