From 2af0807abd832632d04c35ec672918f753e96659 Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sat, 18 Jan 2025 22:36:57 -0500 Subject: [PATCH] cam manager, hopefully somewhat realtime --- controller/src/main.rs | 126 +++++++++++++++++++++++++++++++++++++---- 1 file changed, 114 insertions(+), 12 deletions(-) diff --git a/controller/src/main.rs b/controller/src/main.rs index b02531a..f0d5df1 100644 --- a/controller/src/main.rs +++ b/controller/src/main.rs @@ -19,7 +19,8 @@ use cyw43_pio::{PioSpi, DEFAULT_CLOCK_DIVIDER}; use embassy_rp::i2c::{Async, I2c}; use embassy_rp::multicore::{spawn_core1, Stack}; use embassy_rp::pwm::{self, Pwm}; -use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; +use embassy_sync::blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}; +use embassy_sync::blocking_mutex::{Mutex, ThreadModeMutex}; use embassy_sync::channel::Channel; use embedded_hal_bus::i2c::RefCellDevice; use fixed::FixedU16; @@ -30,7 +31,7 @@ use embassy_net::tcp::TcpSocket; use embassy_net::{Config, StackResources}; use embassy_rp::bind_interrupts; use embassy_rp::clocks::RoscRng; -use embassy_rp::gpio::{Level, Output}; +use embassy_rp::gpio::{Input, Level, Output, Pull}; use embassy_rp::peripherals::{DMA_CH0, PIO0, USB}; use embassy_rp::pio::{InterruptHandler, Pio}; use embassy_rp::usb::Driver; @@ -86,16 +87,21 @@ async fn main(spawner: Spawner) { let driver = Driver::new(p.USB, Irqs); spawner.spawn(logger_task(driver)).unwrap(); - let mut flipper = Output::new(p.PIN_22, Level::Low); + let mut limit_switch = Input::new(p.PIN_22, Pull::Up); + + let mut d: pwm::Config = Default::default(); + d.divider = 40.into(); + d.top = 62500; // 20ms + d.compare_b = 4687; // 1.5ms + d.compare_a = 4687; // 1.5ms + let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, d.clone()); + + let mut f: pwm::Config = Default::default(); + f.divider = 40.into(); + f.top = 62500; // 20ms + f.compare_b = 4687; // 1.5ms + let flip_pwm = Pwm::new_output_b(p.PWM_SLICE0, p.PIN_17, f.clone()); - let mut c: pwm::Config = Default::default(); - c.divider = 40.into(); - c.top = 62500; // 20ms - c.compare_b = 4687; // 1.5ms - c.compare_a = 4687; // 1.5ms - let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone()); - flipper.set_high(); - let sda = p.PIN_20; let scl = p.PIN_21; let config = embassy_rp::i2c::Config::default(); @@ -179,6 +185,12 @@ async fn main(spawner: Spawner) { let mut buf = [0; 4096]; let mut telem_buf = [0; 4096]; + let cam_state: RefCell = RefCell::new(CamState::Idle); + + let cam_state_forever = unsafe { transmute(&cam_state) }; // SAFETY: if main exits we shall + // shortly die. + spawner.spawn(cam_manager(flip_pwm, limit_switch, cam_state_forever)).unwrap(); + loop { let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); socket.set_timeout(Some(Duration::from_secs(3))); @@ -190,7 +202,7 @@ async fn main(spawner: Spawner) { c.compare_a = 4687; // 1.5ms drive_pwm.set_config(&c); - flipper.set_low(); + cam_state.replace(CamState::Idle); control.gpio_set(0, false).await; info!("Listening on TCP:1234..."); @@ -220,7 +232,31 @@ async fn main(spawner: Spawner) { ControlPacket::Twist(forward, right) => { info!("left to {}", clamp(forward+right, -1., 1.)); info!("right to {}", clamp(forward-right, -1., 1.)); + + c.compare_a = calc_speed(right + forward); + c.compare_b = calc_speed(right - forward); + drive_pwm.set_config(&c); }, + ControlPacket::Fire => { + cam_state.replace(CamState::Fire); + }, + ControlPacket::Stop => { + c.compare_a = calc_speed(0.); + c.compare_b = calc_speed(0.); + drive_pwm.set_config(&c); + + cam_state.replace(CamState::Idle); + }, + ControlPacket::FireOverride(speed) => { + cam_state.replace(CamState::Override(speed)); + }, + ControlPacket::Arm(enable) => { + if enable { + cam_state.replace(CamState::Charging(None)); + } else { + cam_state.replace(CamState::Idle); + } + } d => {error!("unimplemented: {d:?}")} // TODO } } @@ -241,3 +277,69 @@ async fn main(spawner: Spawner) { } } } + +/// -1 to 1 +fn calc_speed(speed: f32) -> u16 { + const COUNTS_PER_MS: f32 = 3125.; + let speed = speed.clamp(-1., 1.); + + let ms = (speed/2.0)+1.5; + + (COUNTS_PER_MS * ms) as u16 +} + +enum CamState { + Override(f32), + Charging(Option), + Charged, + Fire, + Idle, +} + +#[embassy_executor::task] +async fn cam_manager(mut cam: Pwm<'static>, limit: Input<'static>, cam_state: &'static RefCell) { + loop { + let mut cam_state = cam_state.borrow_mut(); + handle_cam(&mut cam, &mut (*cam_state), &limit); + drop(cam_state); + + Timer::after_micros(500).await; + } +} + +fn handle_cam(cam: &mut Pwm, state: &mut CamState, limit: &Input) { + let speed = match state { + CamState::Override(speed) => { + *speed + }, + CamState::Charging(mut charged) => { + if limit.is_low() { + let became_low = charged.get_or_insert_with(Instant::now); + if became_low.elapsed().as_micros() > 1800 { + *state = CamState::Charged; + 0.0 + } else { + 1.0 + } + } else { + charged = None; + 1.0 + } + }, + CamState::Fire => { + if limit.is_high() { + *state = CamState::Charging(None); + } + 1.0 + }, + CamState::Charged | CamState::Idle => 0.0, + }; + + + + let mut f: pwm::Config = Default::default(); + f.divider = 40.into(); + f.top = 62500; // 20ms + f.compare_b = calc_speed(speed); // 1.5ms + cam.set_config(&f); +}