added weapon enable flag

handles robot restarts better

controller/src/main.rs

@@ -15,11 +15,13 @@ use core::mem::transmute;
 use core::panic::PanicInfo;
 use core::str::from_utf8;
 
+use cyw43::JoinOptions;
 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;
@@ -28,9 +30,9 @@ use log::*;
 use embassy_executor::Spawner;
 use embassy_net::tcp::TcpSocket;
 use embassy_net::{Config, StackResources};
-use embassy_rp::bind_interrupts;
+use embassy_rp::{bind_interrupts, watchdog};
 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,15 +88,20 @@ 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 limit_switch = Input::new(p.PIN_22, Pull::Up);
 
-    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 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 sda = p.PIN_20;
     let scl = p.PIN_21;
@@ -172,13 +179,31 @@ async fn main(spawner: Spawner) {
     defmt::unwrap!(spawner.spawn(net_task(runner)));
 
     // password is not terribly private information
-    control.start_ap_wpa2("cruisecontrol", "dxSk2avMFvsY", 5).await;
+    //control.start_ap_wpa2("cruisecontrol", "dxSk2avMFvsY", 5).await;
+
+    loop {
+        match control
+            .join("cruisecontrol", JoinOptions::new("dxSk2avMFvsY".as_bytes()))
+            .await
+        {
+            Ok(_) => break,
+            Err(err) => {
+                info!("join failed with status={}", err.status);
+            }
+        }
+    }
 
     let mut rx_buffer = [0; 4096];
     let mut tx_buffer = [0; 4096];
     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 +215,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...");
@@ -206,6 +231,7 @@ async fn main(spawner: Spawner) {
             let mut len: [u8; 4] = [0;4];
             if let Err(e) = socket.read_exact(&mut len).await {
                 warn!("read err: {e:?}");
+                break;
             }
             let len = u32::from_be_bytes(len) as usize;
             let data = &mut buf[0..len];
@@ -220,7 +246,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(forward - right);
+                        c.compare_b = calc_speed(forward + right);
+                        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);
+                        } else {
+                            cam_state.replace(CamState::Idle);
+                        }
+                    }
                     d => {error!("unimplemented: {d:?}")} // TODO
                 }
             }
@@ -228,7 +278,7 @@ async fn main(spawner: Spawner) {
             if let Ok((data, _ts)) = CHANNEL.try_receive() {
                 let telem = TelemetryPacket {
                     sensors: data,
-                    voltage: 0.0, // TODO
+                    cam_state: cam_state.borrow().to_telem(),
                 };
 
                 if let Ok(telem) = postcard::to_slice(&telem, &mut telem_buf) {
@@ -241,3 +291,75 @@ 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
+}
+
+#[derive(Debug)]
+enum CamState {
+    Override(f32),
+    Charging,
+    Charged,
+    Fire,
+    Idle,
+}
+
+impl CamState {
+    fn to_telem(&self) -> common::CamState {
+        match self {
+            Self::Override(_) | Self::Fire => common::CamState::Firing,
+            Self::Charging => common::CamState::Charging,
+            Self::Charged => common::CamState::Charged,
+            Self::Idle => common::CamState::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 => {
+            if limit.is_low() {
+                *state = CamState::Charged;
+                0.0
+            } else {
+                1.0
+            }
+        },
+        CamState::Fire => {
+            if limit.is_high() {
+                *state = CamState::Charging;
+            }
+            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);
+}

interface/src/main.rs

@@ -1,12 +1,12 @@
 #![feature(iter_collect_into)]
-use std::{thread::sleep, time::Duration};
+use std::{ops::ControlFlow, result, sync::Arc, thread::sleep, time::Duration};
 
 use anyhow::{Context, Ok, Result};
 use common::{ControlPacket, TelemetryPacket};
 use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
 use pitch_detection::{detector::{mcleod::McLeodDetector, PitchDetector}, utils};
 use rust_music_theory::note::{Note, NoteLetter, Pitch, Tuning};
-use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, sync::mpsc};
+use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, spawn, sync::{self, mpsc, RwLock}, time::timeout};
 
 fn main() -> Result<()> {
     // assumes pulseaudio system with f32 samples and 2204 sample packets
@@ -43,9 +43,11 @@ fn main() -> Result<()> {
 
     let executor = tokio::runtime::Runtime::new()?;
 
+    //let latest_telem = Arc::new(RwLock::new(None));
 
     let control = executor.block_on(async {
         let cruisecontrol = TcpStream::connect("169.254.1.1:1234").await?;
+        println!("connected");
         cruisecontrol.set_nodelay(true)?;
         let (telem, control) = cruisecontrol.into_split();
 
@@ -77,55 +79,173 @@ async fn telemetry_handler(mut telem: OwnedReadHalf) -> Result<()> {
 
 async fn controller(mut notes: mpsc::Receiver<(Option<pitch_detection::Pitch<f32>>, f32)>, controller: OwnedWriteHalf) -> Result<()> {
     let mut controller = BufWriter::new(controller);
+    //send_packet(&mut controller, ControlPacket::Arm(true)).await?;
+    //println!("armed flipper");
+
+    //let (auto_tx, mut auto_rx) = mpsc::channel(1);
+    //let mut auto = None;
+
+    //auto = Some(spawn(seek))
+
     loop {
         let mut control = ControlPacket::Stop;
 
-        let (note,vol) = notes.recv().await.context("channel closed")?;
+        let result::Result::Ok(note) = timeout(Duration::from_millis(95), notes.recv()).await else {
+            println!("timeout");
+            send_packet(&mut controller, ControlPacket::Stop).await?;
+            continue;
+        };
+
+        let (note,vol) = note.context("channel closed")?;
         if let Some(note) = note {
-            //dbg!(note.frequency);
-            //dbg!(note.clarity);
-            //dbg!(vol);
-            let note = Note::from_freq(note.frequency, Tuning::EqualTemperament);
-            match note.pitch {
-                Pitch { letter: NoteLetter::A, accidental: 0}  => {
-                    println!("forward");
-                    control = ControlPacket::Twist(1.0, 0.0);
-                }
-                Pitch { letter: NoteLetter::A, accidental: 1}  => {
-                    println!("backward");
-                    control = ControlPacket::Twist(-1.0, 0.0);
-                }
-                Pitch { letter: NoteLetter::C, accidental: 0}  => {
-                    println!("right");
-                    control = ControlPacket::Twist(0.0, 1.0);
-                }
-                Pitch { letter: NoteLetter::C, accidental: 1}  => {
-                    println!("left");
-                    control = ControlPacket::Twist(0.0, -1.0);
-                }
-                Pitch { letter: NoteLetter::G, accidental: 0}  => {
-                    println!("fire");
-                    control = ControlPacket::Fire;
-                }
-                Pitch { letter: NoteLetter::E, accidental: 0}  => {
-                    println!("fire");
-                    control = ControlPacket::Fire;
-                }
-                _pitch => {
-                    if vol > 3000. {
-                        println!("fire");
-                        control = ControlPacket::FireOverride(1.0);
-                    } else {
-                        //dbg!(pitch);
-                    }
+            dbg!(note.frequency);
+            if let ControlFlow::Break(_) = sax_control(&mut control, vol, &note) {
+                if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note) {
+                    continue;
                 }
             }
 
-
-            let control: heapless::Vec<u8, 2048> = postcard::to_vec(&control)?;
-            controller.write_u32(control.len() as u32).await?;
-            controller.write_all(&control).await?;
-            controller.flush().await?;
+            send_packet(&mut controller, control).await?;
+        } else {
+            send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
         }
     }
 }
+
+/// Weapon enabled
+const ARMED: bool = true;
+
+fn sax_control(control: &mut ControlPacket, vol: f32, note: &pitch_detection::Pitch<f32>) -> ControlFlow<()> {
+    if note.frequency < 150. {
+        println!("too low");
+        return ControlFlow::Break(());
+    }
+    if note.frequency > 270. {
+        println!("too high");
+        return ControlFlow::Break(());
+    }
+    let note = Note::from_freq(note.frequency, Tuning::EqualTemperament);
+    //dbg!(note.clarity);
+    //dbg!(vol);
+    match note.pitch {
+        Pitch { letter: NoteLetter::C, accidental: 1}  => {
+            println!("forward");
+            *control = ControlPacket::Twist(1.0, 0.0);
+        }
+        //Pitch { letter: NoteLetter::A, accidental: 1}  => {
+        //    println!("backward");
+        //    *control = ControlPacket::Twist(-1.0, 0.0);
+        //}
+        Pitch { letter: NoteLetter::B, accidental: 0}  => {
+            println!("right");
+            *control = ControlPacket::Twist(0.0, 1.0);
+        }
+        Pitch { letter: NoteLetter::A, accidental: 0}  => {
+            println!("left");
+            *control = ControlPacket::Twist(0.0, -1.0);
+        }
+        Pitch { letter: NoteLetter::G, accidental: 1}  => {
+            if ARMED {
+                println!("fire");
+                *control = ControlPacket::Fire;
+            }
+        }
+        Pitch { letter: NoteLetter::F, accidental: 1}  => {
+            println!("stop flat");
+            *control = ControlPacket::Stop;
+        }
+        pitch => {
+            if vol > 3000. {
+                println!("rly loud");
+                //control = ControlPacket::FireOverride(1.0);
+                dbg!(pitch);
+            } else {
+                dbg!(pitch);
+            }
+        }
+    }
+    ControlFlow::Continue(())
+}
+
+
+fn recorder_control(control: &mut ControlPacket, vol: f32, note: pitch_detection::Pitch<f32>) -> ControlFlow<()> {
+    if note.frequency < 300. {
+        println!("too low");
+        return ControlFlow::Break(());
+    }
+    if note.frequency > 600. {
+        println!("too high");
+        return ControlFlow::Break(());
+    }
+    let note = Note::from_freq(note.frequency, Tuning::EqualTemperament);
+    //dbg!(note.clarity);
+    //dbg!(vol);
+    match note.pitch {
+        Pitch { letter: NoteLetter::A, accidental: 0}  => {
+            println!("forward");
+            *control = ControlPacket::Twist(1.0, 0.0);
+        }
+        Pitch { letter: NoteLetter::A, accidental: 1}  => {
+            println!("backward");
+            *control = ControlPacket::Twist(-1.0, 0.0);
+        }
+        Pitch { letter: NoteLetter::C, accidental: 0}  => {
+            println!("right");
+            *control = ControlPacket::Twist(0.0, 1.0);
+        }
+        Pitch { letter: NoteLetter::C, accidental: 1}  => {
+            println!("left");
+            *control = ControlPacket::Twist(0.0, -1.0);
+        }
+        Pitch { letter: NoteLetter::G, accidental: 0|1}  => {
+            if ARMED {
+                println!("fire");
+                *control = ControlPacket::Fire;
+            }
+        }
+        Pitch { letter: NoteLetter::F, accidental: 1}  => {
+            println!("stop flat");
+            *control = ControlPacket::Stop;
+        }
+        Pitch { letter: NoteLetter::F, accidental: 0}  => {
+            println!("stop");
+            *control = ControlPacket::Stop;
+        }
+        Pitch { letter: NoteLetter::E, accidental: 0}  => {
+            println!("stop");
+            *control = ControlPacket::Stop;
+            //if let result::Result::Ok(command) = auto_rx.try_recv() {
+            //    control = command;
+            //}
+        }
+        pitch => {
+            if vol > 3000. {
+                println!("rly loud");
+                //control = ControlPacket::FireOverride(1.0);
+            } else {
+                dbg!(pitch);
+            }
+        }
+    }
+    ControlFlow::Continue(())
+}
+
+async fn send_packet(controller: &mut BufWriter<OwnedWriteHalf>, control: ControlPacket) -> Result<(), anyhow::Error> {
+    let control: heapless::Vec<u8, 2048> = postcard::to_vec(&control)?;
+    controller.write_u32(control.len() as u32).await?;
+    controller.write_all(&control).await?;
+    controller.flush().await?;
+    Ok(())
+}
+
+//async fn seek(data: TelemetryPacket) -> Result<()>{
+//
+//    let left_tof = data.sensors.tof1.context("missing tof1")?;
+//    let right_tof = data.sensors.tof2.context("missing tof2")?;
+//
+//    /// Distance (mm) where both tofs point at the same thing
+//    const CONVERGANCE_DISTANCE: u16 = 80;
+//    const DETECT_DISTANCE: u16 = 380;
+//
+//
+//}