controller/src/main.rs

@@ -15,13 +15,11 @@ 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, ThreadModeRawMutex};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
-use embassy_sync::blocking_mutex::{Mutex, ThreadModeMutex};
 use embassy_sync::channel::Channel;
 use embedded_hal_bus::i2c::RefCellDevice;
 use fixed::FixedU16;
@@ -30,9 +28,9 @@ use log::*;
 use embassy_executor::Spawner;
 use embassy_net::tcp::TcpSocket;
 use embassy_net::{Config, StackResources};
-use embassy_rp::{bind_interrupts, watchdog};
+use embassy_rp::bind_interrupts;
 use embassy_rp::clocks::RoscRng;
-use embassy_rp::gpio::{Input, Level, Output, Pull};
+use embassy_rp::gpio::{Level, Output};
 use embassy_rp::peripherals::{DMA_CH0, PIO0, USB};
 use embassy_rp::pio::{InterruptHandler, Pio};
 use embassy_rp::usb::Driver;
@@ -88,20 +86,15 @@ async fn main(spawner: Spawner) {
     let driver = Driver::new(p.USB, Irqs);
     spawner.spawn(logger_task(driver)).unwrap();
 
-    let limit_switch = Input::new(p.PIN_22, Pull::Up);
+    let mut flipper = Output::new(p.PIN_22, Level::Low);
 
-    let mut d: pwm::Config = Default::default();
+    let mut c: pwm::Config = Default::default();
-    d.divider = 40.into();
+    c.divider = 40.into();
-    d.top = 62500; // 20ms
+    c.top = 62500; // 20ms
-    d.compare_b = 4687; // 1.5ms
+    c.compare_b = 4687; // 1.5ms
-    d.compare_a = 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, d.clone());
+    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone());
-
+    flipper.set_high();
-    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;
@@ -179,31 +172,13 @@ 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)));
@@ -215,7 +190,7 @@ async fn main(spawner: Spawner) {
         c.compare_a = 4687; // 1.5ms
         drive_pwm.set_config(&c);
 
-        cam_state.replace(CamState::Idle);
+        flipper.set_low();
 
         control.gpio_set(0, false).await;
         info!("Listening on TCP:1234...");
@@ -231,7 +206,6 @@ 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];
@@ -246,31 +220,7 @@ 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
                 }
             }
@@ -278,7 +228,7 @@ async fn main(spawner: Spawner) {
             if let Ok((data, _ts)) = CHANNEL.try_receive() {
                 let telem = TelemetryPacket {
                     sensors: data,
-                    cam_state: cam_state.borrow().to_telem(),
+                    voltage: 0.0, // TODO
                 };
 
                 if let Ok(telem) = postcard::to_slice(&telem, &mut telem_buf) {
@@ -291,75 +241,3 @@ 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::{ops::ControlFlow, result, sync::Arc, thread::sleep, time::Duration};
+use std::{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}, spawn, sync::{self, mpsc, RwLock}, time::timeout};
+use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, sync::mpsc};
 
 fn main() -> Result<()> {
     // assumes pulseaudio system with f32 samples and 2204 sample packets
@@ -43,11 +43,9 @@ 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();
 
@@ -79,173 +77,55 @@ 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 result::Result::Ok(note) = timeout(Duration::from_millis(95), notes.recv()).await else {
+        let (note,vol) = notes.recv().await.context("channel closed")?;
-            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.frequency);
-            if let ControlFlow::Break(_) = sax_control(&mut control, vol, &note) {
+            //dbg!(note.clarity);
-                if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note) {
+            //dbg!(vol);
-                    continue;
+            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);
+                    }
                 }
             }
 
-            send_packet(&mut controller, control).await?;
+
-        } else {
+            let control: heapless::Vec<u8, 2048> = postcard::to_vec(&control)?;
-            send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
+            controller.write_u32(control.len() as u32).await?;
+            controller.write_all(&control).await?;
+            controller.flush().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;
-//
-//
-//}