From 66e589bc26ed6f76ad19be4067ce45e2d4523f1c Mon Sep 17 00:00:00 2001 From: Naxdy Date: Mon, 8 May 2023 16:15:59 +0200 Subject: [PATCH] naxbox draft --- src/gccstate.rs | 362 +++++++++++++++++++++++++++++++++++++++++++++++- src/input.rs | 47 +++++++ src/joybus.rs | 75 +++++----- src/main.rs | 29 +++- 4 files changed, 463 insertions(+), 50 deletions(-) create mode 100644 src/input.rs diff --git a/src/gccstate.rs b/src/gccstate.rs index 7460964..fa3797e 100644 --- a/src/gccstate.rs +++ b/src/gccstate.rs @@ -1,3 +1,59 @@ +use embedded_hal::digital::v2::InputPin; +use rp2040_hal::gpio::{bank0::*, Pin, PinId, PullUp, PullUpInput}; + +macro_rules! determine_stick_state { + ($previous_state:ident, $current_state:ident, $button:ident) => { + match $previous_state.$button { + true => match $current_state.$button { + true => StickButtonState::UNCHANGED, + false => StickButtonState::RELEASED, + }, + false => match $current_state.$button { + true => StickButtonState::PRESSED, + false => StickButtonState::UNCHANGED, + }, + } + }; +} + +macro_rules! determine_positive_stick_axis { + ($stick_state:ident, $modifier_state:ident, $gccstate:ident, $pin_state:ident, $axis:ident, $neg_axis:ident) => { + match $stick_state { + StickButtonState::PRESSED => { + $gccstate.$axis = match $modifier_state { + ModifierState::MOD1 => 159, // 127 + 32 + ModifierState::MOD2 => 191, // 127 + 64 + ModifierState::NONE => 255, // 127 + 128 + } + } + StickButtonState::RELEASED => match $pin_state.$neg_axis { + true => {} + false => $gccstate.$axis = 127, + }, + StickButtonState::UNCHANGED => {} + } + }; +} + +macro_rules! determine_negative_stick_axis { + ($stick_state:ident, $modifier_state:ident, $gccstate:ident, $pin_state:ident, $axis:ident, $pos_axis:ident) => { + match $stick_state { + StickButtonState::PRESSED => { + $gccstate.$axis = match $modifier_state { + ModifierState::MOD1 => 95, // 127 - 32 + ModifierState::MOD2 => 63, // 127 - 64 + ModifierState::NONE => 0, // 127 - 128 + } + } + StickButtonState::RELEASED => match $pin_state.$pos_axis { + true => {} + false => $gccstate.$axis = 127, + }, + StickButtonState::UNCHANGED => {} + } + }; +} + #[derive(Debug, Clone, Copy)] pub struct GccState { pub a: bool, @@ -27,7 +83,7 @@ impl GccState { y: false, start: false, dup: false, - dleft: true, + dleft: false, dright: false, ddown: false, l: false, @@ -70,3 +126,307 @@ impl From for [u8; 8] { response } } + +pub struct PinMap { + up: Pin, + down: Pin, + left: Pin, + right: Pin, + q: Pin, + e: Pin, + shift: Pin, + a: Pin, + b: Pin, + x: Pin, + y: Pin, + l: Pin, + r: Pin, + zl: Pin, + zr: Pin, + start: Pin, + cup: Pin, + cleft: Pin, + cright: Pin, + cdown: Pin, + dup: Pin, + dleft: Pin, + dright: Pin, + ddown: Pin, +} + +impl PinMap { + pub fn new(pins: rp2040_hal::gpio::Pins) -> PinMap { + PinMap { + up: pins.gpio0.into_pull_up_input(), + down: pins.gpio2.into_pull_up_input(), + left: pins.gpio1.into_pull_up_input(), + right: pins.gpio3.into_pull_up_input(), + q: pins.gpio5.into_pull_up_input(), + e: pins.gpio6.into_pull_up_input(), + shift: pins.gpio8.into_pull_up_input(), + a: pins.gpio9.into_pull_up_input(), + b: pins.gpio11.into_pull_up_input(), + x: pins.gpio13.into_pull_up_input(), + y: pins.gpio12.into_pull_up_input(), + l: pins.gpio10.into_pull_up_input(), + r: pins.gpio14.into_pull_up_input(), + zl: pins.gpio16.into_pull_up_input(), + zr: pins.gpio15.into_pull_up_input(), + start: pins.gpio7.into_pull_up_input(), + cup: pins.gpio17.into_pull_up_input(), + cleft: pins.gpio23.into_pull_up_input(), + cright: pins.gpio21.into_pull_up_input(), + cdown: pins.gpio19.into_pull_up_input(), + dup: pins.gpio4.into_pull_up_input(), + dleft: pins.gpio18.into_pull_up_input(), + dright: pins.gpio20.into_pull_up_input(), + ddown: pins.gpio22.into_pull_up_input(), + } + } +} + +pub struct PinState { + up: bool, + down: bool, + left: bool, + right: bool, + q: bool, + e: bool, + shift: bool, + a: bool, + b: bool, + x: bool, + y: bool, + l: bool, + r: bool, + zl: bool, + zr: bool, + start: bool, + cup: bool, + cleft: bool, + cright: bool, + cdown: bool, + dup: bool, + dleft: bool, + dright: bool, + ddown: bool, +} + +impl PinState { + pub fn new() -> PinState { + PinState { + up: false, + down: false, + left: false, + right: false, + q: false, + e: false, + shift: false, + a: false, + b: false, + x: false, + y: false, + l: false, + r: false, + zl: false, + zr: false, + start: false, + cup: false, + cleft: false, + cright: false, + cdown: false, + dup: false, + dleft: false, + dright: false, + ddown: false, + } + } +} + +impl From<&PinMap> for PinState { + fn from(value: &PinMap) -> Self { + PinState { + up: value.up.is_low().unwrap(), + down: value.down.is_low().unwrap(), + left: value.left.is_low().unwrap(), + right: value.right.is_low().unwrap(), + q: value.q.is_low().unwrap(), + e: value.e.is_low().unwrap(), + shift: value.shift.is_low().unwrap(), + a: value.a.is_low().unwrap(), + b: value.b.is_low().unwrap(), + x: value.x.is_low().unwrap(), + y: value.y.is_low().unwrap(), + l: value.l.is_low().unwrap(), + r: value.r.is_low().unwrap(), + zl: value.zl.is_low().unwrap(), + zr: value.zr.is_low().unwrap(), + start: value.start.is_low().unwrap(), + cup: value.cup.is_low().unwrap(), + cleft: value.cleft.is_low().unwrap(), + cright: value.cright.is_low().unwrap(), + cdown: value.cdown.is_low().unwrap(), + dup: value.dup.is_low().unwrap(), + dleft: value.dleft.is_low().unwrap(), + dright: value.dright.is_low().unwrap(), + ddown: value.ddown.is_low().unwrap(), + } + } +} + +enum StickButtonState { + PRESSED, + RELEASED, + UNCHANGED, +} + +enum ModifierState { + NONE, + MOD1, + MOD2, +} + +pub fn pinstate_to_gccstate( + previous_pin_state: &PinState, + current_pin_state: &PinState, + current_gcc_state: &GccState, +) -> GccState { + let mut gccstate = current_gcc_state.clone(); + + // easy buttons + gccstate.start = current_pin_state.start; + gccstate.a = current_pin_state.a; + gccstate.b = current_pin_state.b; + gccstate.x = current_pin_state.x; + gccstate.y = current_pin_state.y; + gccstate.l = current_pin_state.l; + gccstate.r = current_pin_state.r || current_pin_state.ddown; + gccstate.z = current_pin_state.zl || current_pin_state.zr || current_pin_state.dleft; + + let stick_to_dpad = current_pin_state.q || current_pin_state.e; + + if stick_to_dpad { + gccstate.lsticky = 127; + gccstate.lstickx = 127; + + gccstate.dup = current_pin_state.up; + gccstate.ddown = current_pin_state.down; + gccstate.dleft = current_pin_state.left; + gccstate.dright = current_pin_state.right; + + return gccstate; + } + + // MOD1 = soft modifier, makes stick go half length + // MOD2 = hard modifier, makes stick go quarter length + let modifier_state = match current_pin_state.shift || current_pin_state.dup { + true => ModifierState::MOD1, + false => match current_pin_state.ddown { + true => ModifierState::MOD2, + false => ModifierState::NONE, + }, + }; + + // determine down / up presses for stick buttons + let lstick_up_state = determine_stick_state!(previous_pin_state, current_pin_state, up); + let lstick_down_state = determine_stick_state!(previous_pin_state, current_pin_state, down); + let lstick_left_state = determine_stick_state!(previous_pin_state, current_pin_state, left); + let lstick_right_state = determine_stick_state!(previous_pin_state, current_pin_state, right); + + let cstick_up_state = determine_stick_state!(previous_pin_state, current_pin_state, cup); + let cstick_down_state = determine_stick_state!(previous_pin_state, current_pin_state, cdown); + let cstick_left_state = determine_stick_state!(previous_pin_state, current_pin_state, cleft); + let cstick_right_state = determine_stick_state!(previous_pin_state, current_pin_state, cright); + + determine_positive_stick_axis!( + lstick_up_state, + modifier_state, + gccstate, + current_pin_state, + lsticky, + down + ); + determine_negative_stick_axis!( + lstick_down_state, + modifier_state, + gccstate, + current_pin_state, + lsticky, + up + ); + determine_positive_stick_axis!( + lstick_right_state, + modifier_state, + gccstate, + current_pin_state, + lstickx, + left + ); + determine_negative_stick_axis!( + lstick_left_state, + modifier_state, + gccstate, + current_pin_state, + lstickx, + right + ); + + let cstick_modifier_state = ModifierState::NONE; + + determine_positive_stick_axis!( + cstick_up_state, + cstick_modifier_state, + gccstate, + current_pin_state, + rsticky, + cdown + ); + determine_negative_stick_axis!( + cstick_down_state, + cstick_modifier_state, + gccstate, + current_pin_state, + rsticky, + cup + ); + determine_positive_stick_axis!( + cstick_right_state, + cstick_modifier_state, + gccstate, + current_pin_state, + rstickx, + cleft + ); + determine_negative_stick_axis!( + cstick_left_state, + cstick_modifier_state, + gccstate, + current_pin_state, + rstickx, + cright + ); + + // clamp lstickx if lsticky is 0 or 255 for uptilt and downtilt angles + if gccstate.lsticky <= 1 || gccstate.lsticky >= 254 { + gccstate.lstickx = gccstate.lstickx.max(53).min(201); + } else { + gccstate.lstickx = match gccstate.lstickx { + 201 => 255, + 53 => 0, + _ => gccstate.lstickx, + } + } + + // do the same for cstick + if gccstate.rsticky <= 1 || gccstate.rsticky >= 254 { + gccstate.rstickx = gccstate.rstickx.max(53).min(201); + } else { + gccstate.rstickx = match gccstate.rstickx { + 201 => 255, + 53 => 0, + _ => gccstate.rstickx, + } + } + + gccstate +} diff --git a/src/input.rs b/src/input.rs new file mode 100644 index 0000000..416be3c --- /dev/null +++ b/src/input.rs @@ -0,0 +1,47 @@ +use rp2040_hal::Sio; + +use crate::gccstate::{pinstate_to_gccstate, GccState, PinMap, PinState}; + +pub static mut CURRENT_STATE: GccState = GccState { + a: false, + b: false, + x: false, + y: false, + start: false, + dup: false, + dleft: false, + dright: false, + ddown: false, + l: false, + r: false, + z: true, + lstickx: 127, + lsticky: 127, + rstickx: 127, + rsticky: 127, +}; + +pub fn input_loop() -> ! { + let mut pac = unsafe { rp2040_hal::pac::Peripherals::steal() }; + let sio = Sio::new(pac.SIO); + let pins = rp2040_hal::gpio::Pins::new( + pac.IO_BANK0, + pac.PADS_BANK0, + sio.gpio_bank0, + &mut pac.RESETS, + ); + + let mut previous_pin_state = PinState::new(); + let pin_map = PinMap::new(pins); + + loop { + let current_pin_state: PinState = (&pin_map).into(); + + unsafe { + CURRENT_STATE = + pinstate_to_gccstate(&previous_pin_state, ¤t_pin_state, &CURRENT_STATE); + } + + previous_pin_state = current_pin_state; + } +} diff --git a/src/joybus.rs b/src/joybus.rs index 4af2eac..fa64188 100644 --- a/src/joybus.rs +++ b/src/joybus.rs @@ -1,5 +1,6 @@ use cortex_m::delay::Delay; use embedded_hal::digital::v2::{InputPin, OutputPin}; +use rp2040_hal::gpio::{bank0::BankPinId, PinId}; use rp_pico::{ hal::{ pio::{ @@ -13,6 +14,7 @@ use rp_pico::{ use crate::{ gccstate::GccState, + input::CURRENT_STATE, pio::{to_pio_response, PioResponse}, }; @@ -74,14 +76,15 @@ where } #[link_section = ".time_critical.datatransfer"] -pub fn datatransfer( - pins: rp_pico::Pins, +pub fn datatransfer( + datapin: rp2040_hal::gpio::Pin, pio0: PIO0, mut resets: &mut RESETS, mut delay: Delay, -) -> ! { - let datapin = pins.gpio28.into_pull_up_input(); - +) -> ! +where + I: BankPinId + PinId, +{ let datapin = datapin.into_mode::(); let (mut pio, sm0, _, _, _) = pio0.split(&mut resets); @@ -102,10 +105,6 @@ pub fn datatransfer( let mut s = sm.start(); - let abutton = pins.gpio0.into_pull_up_input(); - - let mut ledpin = pins.led.into_push_pull_output(); - loop { let data = loop { match rx.read() { @@ -118,47 +117,38 @@ pub fn datatransfer( // poll if data == 0x40 { - let mut state = GccState::new(); + unsafe { + let state = CURRENT_STATE.clone(); - state.a = !abutton.is_high().unwrap(); + let bitstate = state.into(); - match state.a { - true => ledpin.set_high().unwrap(), - false => ledpin.set_low().unwrap(), - } + let pio_response: PioResponse<5> = to_pio_response(bitstate); - if state.a { - state.lstickx = 255; - } + while rx.read().is_none() {} - let bitstate = state.into(); + let _rumble = loop { + match rx.read() { + Some(x) => break x as u8, + None => continue, + } + }; - let pio_response: PioResponse<5> = to_pio_response(bitstate); + delay.delay_us(8); - while rx.read().is_none() {} + let mut st = s.stop(); + st.exec_instruction(pio::Instruction { + operands: pio::InstructionOperands::JMP { + condition: pio::JmpCondition::Always, + address: offset + offset_outmode as u8, + }, + delay: 0, + side_set: None, + }); + s = st.start(); - let _rumble = loop { - match rx.read() { - Some(x) => break x as u8, - None => continue, + for i in 0..pio_response.len() { + while !tx.write((*pio_response)[i]) {} } - }; - - delay.delay_us(8); - - let mut st = s.stop(); - st.exec_instruction(pio::Instruction { - operands: pio::InstructionOperands::JMP { - condition: pio::JmpCondition::Always, - address: offset + offset_outmode as u8, - }, - delay: 0, - side_set: None, - }); - s = st.start(); - - for i in 0..pio_response.len() { - while !tx.write((*pio_response)[i]) {} } // origin } else if data == 0x41 { @@ -204,7 +194,6 @@ pub fn datatransfer( } // something else (probably snippet of some other message) } else { - ledpin.set_high().unwrap(); let (st, installed) = s.uninit(rx, tx); (sm, rx, tx) = diff --git a/src/main.rs b/src/main.rs index 138909d..26c4739 100644 --- a/src/main.rs +++ b/src/main.rs @@ -10,31 +10,35 @@ use defmt_rtt as _; use panic_probe as _; +use rp2040_hal::multicore::{Multicore, Stack}; // Provide an alias for our BSP so we can switch targets quickly. // Uncomment the BSP you included in Cargo.toml, the rest of the code does not need to change. -use rp_pico as bsp; +use rp2040_hal as bsp; // use sparkfun_pro_micro_rp2040 as bsp; -use bsp::hal::{ +use bsp::{ clocks::{init_clocks_and_plls, Clock}, pac, sio::Sio, watchdog::Watchdog, }; -use crate::joybus::datatransfer; +use crate::{input::input_loop, joybus::datatransfer}; mod gccstate; +mod input; mod joybus; mod pio; +static mut CORE1_STACK: Stack<4096> = Stack::new(); + #[entry] fn main() -> ! { info!("Program start"); let mut pac = pac::Peripherals::take().unwrap(); let core = pac::CorePeripherals::take().unwrap(); let mut watchdog = Watchdog::new(pac.WATCHDOG); - let sio = Sio::new(pac.SIO); + let mut sio = Sio::new(pac.SIO); // External high-speed crystal on the pico board is 12Mhz let external_xtal_freq_hz = 12_000_000u32; @@ -52,7 +56,7 @@ fn main() -> ! { let delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); - let pins = bsp::Pins::new( + let pins = rp2040_hal::gpio::Pins::new( pac.IO_BANK0, pac.PADS_BANK0, sio.gpio_bank0, @@ -68,7 +72,20 @@ fn main() -> ! { // let datapin = pins.gpio0.into_pull_up_input(); - datatransfer(pins, pac.PIO0, &mut pac.RESETS, delay) + let mut mc = Multicore::new(&mut pac.PSM, &mut pac.PPB, &mut sio.fifo); + let cores = mc.cores(); + let core1 = &mut cores[1]; + + let _input_loop = core1 + .spawn(unsafe { &mut CORE1_STACK.mem }, input_loop) + .unwrap(); + + datatransfer( + pins.gpio28.into_pull_up_input(), + pac.PIO0, + &mut pac.RESETS, + delay, + ) } // End of file