cam manager, hopefully somewhat realtime

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<CamState> = 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<Instant>),
+    Charged,
+    Fire,
+    Idle,
+}
+
+#[embassy_executor::task]
+async fn cam_manager(mut cam: Pwm<'static>, limit: Input<'static>, cam_state: &'static RefCell<CamState>) {
+    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);
+}