switched to a state instead of change architecture

southbridge currently hangs, embassy does not make
such things easy to diagnose

common/src/lib.rs

@@ -1,7 +1,7 @@
 //! Data types shared between the northbridge and southbridge for serial communications
 #![no_std]
 
-use nalgebra::Vector3;
+use nalgebra::{SimdPartialOrd, Vector3};
 use serde::{Deserialize, Serialize};
 
 pub const BAUDRATE: u32 = 115200;
@@ -20,9 +20,34 @@ pub enum Command {
     Disable,
 }
 
+#[derive(Serialize, Deserialize, Debug, Clone)]
+pub struct RobotState {
+    /// forward. clockwise
+    pub drive: (f32,f32),
+    /// RGB led brightness
+    pub underglow: Vector3<u8>,
+}
+
+impl RobotState {
+    /// limit exertion to factor of full output (0 to 1)
+    pub fn brownout(self, factor: f32) -> Self {
+        Self { 
+            drive: (
+                   self.drive.0.clamp(-factor, factor), 
+                    self.drive.1.clamp(-factor, factor), 
+                   ),
+            underglow: self.underglow.simd_min(Vector3::repeat((factor * u8::MAX as f32) as u8))
+        } 
+    }
+
+    /// Safe command to send in uncertian states
+    pub fn stopped() -> Self {
+        Self { drive: (0.0, 0.0), underglow: Vector3::new(100, 0, 0) }
+    }
+}
+
 #[derive(Serialize, Deserialize, Debug)]
 pub struct Response {
-    pub enabled: bool,
     pub sensor_data: Option<SensorData>,
     pub uptime_micros: u64,
 }

interface/src/main.rs

@@ -1,7 +1,7 @@
 use std::time::Duration;
 
 use anyhow::Context;
-use common::{Command, SensorData};
+use common::{RobotState, SensorData};
 use gilrs::{Axis, Event, Gilrs};
 use nalgebra::Vector3;
 use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}};
@@ -18,7 +18,7 @@ async fn main() -> anyhow::Result<()> {
     let a = gamepad.button_code(gilrs::Button::South).context("no a")?;
     let mut last_a = false;
 
-    let server = "130.215.211.79:3322";
+    let server = "pelican.dyn.wpi.edu:3322";
     let mut robot = TcpStream::connect(server).await?;
     robot.set_nodelay(true)?;
 
@@ -66,26 +66,20 @@ async fn main() -> anyhow::Result<()> {
             ly = 0.0;
             rx = 0.0;
         }
+        let black = Vector3::repeat(0u8);
+        let green = Vector3::new(254u8, 0, 254);
+        let color = if a {green} else {black};
 
-        let cmd = Command::Twist(ly, rx);
+        let cmd = RobotState {
-        println!("twist");
+            drive: (ly, rx),
+            underglow: color,
+            
+        };
 
         let encoded = postcard::to_stdvec(&cmd)?;
         robot_controller.write_u32(encoded.len() as u32).await?;
         robot_controller.write_all(&encoded).await?;
 
-        if a != last_a {
-            let black = Vector3::repeat(0u8);
-            let green = Vector3::new(254u8, 0, 254);
-            let cmd = Command::SetLed(if a {green} else {black});
-            let encoded = postcard::to_stdvec(&cmd)?;
-            robot_controller.write_u32(encoded.len() as u32).await?;
-            robot_controller.write_all(&encoded).await?;
-            println!("led");
-        }
-        last_a = a;
-
-
         if let Err(e) = robot_controller.flush().await {
             println!("flush fail: {e}, reconnecting");
             robot = TcpStream::connect(server).await?;
@@ -98,7 +92,7 @@ async fn main() -> anyhow::Result<()> {
             robot_controller = BufWriter::new(robot);
         };
 
-        sleep(Duration::from_millis(65)).await;
+        sleep(Duration::from_millis(25)).await;
     }
 }
 

northbridge/src/framed_codec.rs

@@ -1,7 +1,7 @@
 use std::result;
 
 use anyhow::{anyhow, Ok};
-use common::{Command, Response};
+use common::{Response, RobotState};
 use framed::{BoxPayload, FRAME_END_SYMBOL};
 use tokio_util::{bytes::BytesMut, codec::{Decoder, Encoder}};
 
@@ -55,10 +55,10 @@ impl Encoder<Vec<u8>> for FramedCodec {
     }
 }
 
-impl Encoder<Command> for FramedCodec {
+impl Encoder<RobotState> for FramedCodec {
     type Error = anyhow::Error;
 
-    fn encode(&mut self, item: Command, dst: &mut BytesMut) -> Result<(), Self::Error> {
+    fn encode(&mut self, item: RobotState, dst: &mut BytesMut) -> Result<(), Self::Error> {
         let data = postcard::to_stdvec(&item)?;
         Ok(self.encode(data, dst).map_err(|e| anyhow!("encode fail: {e:?}"))?)
     }

northbridge/src/main.rs

@@ -3,19 +3,19 @@
 use std::{sync::Arc, time::Duration};
 
 use anyhow::{Context, Result};
-use common::{Command, Response, SensorData, BAUDRATE};
+use common::{Response, RobotState, SensorData, BAUDRATE};
 use crossbeam::queue::ArrayQueue;
 use framed_codec::FramedCodec;
-use futures::{SinkExt, StreamExt, TryStreamExt};
+use futures::{FutureExt, SinkExt, StreamExt, TryStreamExt};
 use nalgebra::{SimdPartialOrd, SimdValue, Vector3};
-use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Sender, Receiver}, watch::{self }, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}};
+use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver, Sender}, watch::{self, channel }, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}};
 use tokio_serial::SerialPortBuilderExt;
 use tokio_util::codec::Framed;
 
 mod framed_codec;
 
 #[derive(Default)]
-struct RobotState {
+struct RobotInfo {
     bus_voltage: f32,
 }
 
@@ -29,9 +29,13 @@ async fn main() -> Result<()> {
 
     let (mut write, mut read) = Framed::new(serial, FramedCodec::new()).split();
 
-    let commands = Arc::new(ArrayQueue::new(5));
+    loop {
+        write.send(RobotState::stopped()).await?;
+    }
 
-    let state = Arc::new(RwLock::new(RobotState {
+    let (sender, mut command_receiver) = channel(RobotState::stopped());
+
+    let state = Arc::new(RwLock::new(RobotInfo {
         bus_voltage: f32::MAX,
         ..Default::default()
     }));
@@ -39,57 +43,34 @@ async fn main() -> Result<()> {
     tokio::spawn(update_telem(state.clone(), sensor_data.resubscribe()));
 
     let control_telem = sensor_data.resubscribe();
-    let sender = commands.clone();
     tokio::spawn(async move {
         loop {
-            let _ = sender.force_push(Command::Stop);
             if let Err(e) = control(sender.clone(), control_telem.resubscribe()).await {
                 println!("controller exited: {e}");
             }
         }
     });
 
-    println!("starting");
+    tokio::spawn(async move {
-    write.send(Command::Enable).await?;
+        let telem_sender = sensor_sender.clone();
-    write.flush().await?;
+        read.for_each( async |telem| {
-    println!("enable");
+            let Ok(telem) = telem else {return;};
+            let Some(data) = telem.sensor_data else {return;};
+            telem_sender.send(data).unwrap(); 
+        }).await;
+    });
 
     loop {
-        let Some(Ok(data)) = read.next().await else {
+        let _ = timeout(Duration::from_millis(20), command_receiver.changed());
-            continue;
+        let command = command_receiver.borrow();
-        };
-        if data.enabled {
-            write.send(Command::FeedWatchdog).await?;
 
-        } else {
+        println!("sending {command:?}");
-            println!("enabled southbridge");
-            write.send(Command::Enable).await?;
-        }
 
-        if let Some(data) = data.sensor_data {
+        write.send(command.clone()).await?;
-            let _ = sensor_sender.send(data);
-        }
-
-        let voltage = state.read().await.bus_voltage;
-
-        // 100% effort at 9.5v, 0% effort at 7.5v
-        let brownout = ((voltage-7.5)/2.0).clamp(0., 1.);
-
-        let mut comand = Vec::new();
-        for _ in 0..commands.len().min(2) { // while let will never exit
-            if let Some(command) = commands.pop() {
-                comand.push(command);
-            }    
-        }
-        for command in dbg!(comand) {
-            write.feed(command.brownout(brownout)).await?;
-            write.feed(Command::FeedWatchdog).await?;
-        }
-        write.flush().await?;
     }
 }
 
-async fn control(sender: Arc<ArrayQueue<Command>>, mut telem: Receiver<SensorData>) -> Result<()> {
+async fn control(sender: watch::Sender<RobotState>, mut telem: Receiver<SensorData>) -> Result<()> {
     let listener = TcpListener::bind("0.0.0.0:3322").await?;
 
     let (stream, addr) = listener.accept().await?;
@@ -116,7 +97,7 @@ async fn control(sender: Arc<ArrayQueue<Command>>, mut telem: Receiver<SensorDat
             match timeout(Duration::from_millis(150), read.read_u32()).await {
                 Ok(it) => { break it?; },
                 Err(Elapsed) => {
-                    sender.force_push(Command::Stop);
+                    sender.send(RobotState::stopped())?;
                     continue;
                 },
             };
@@ -124,13 +105,13 @@ async fn control(sender: Arc<ArrayQueue<Command>>, mut telem: Receiver<SensorDat
         let mut buf = vec![0; len as usize];
         timeout(Duration::from_millis(100), read.read_exact(&mut buf)).await??;
 
-        let cmd: Command = postcard::from_bytes(&buf)?;
+        let cmd: RobotState = postcard::from_bytes(&buf)?;
 
-        sender.force_push(cmd);
+        sender.send(cmd)?;
     }
 }
 
-async fn update_telem(state: Arc<RwLock<RobotState>>, mut telem: Receiver<SensorData>) -> Result<()> {
+async fn update_telem(state: Arc<RwLock<RobotInfo>>, mut telem: Receiver<SensorData>) -> Result<()> {
     loop {
         let telem = telem.recv().await?;
         println!("sensor {telem:?}");
@@ -144,26 +125,3 @@ async fn update_telem(state: Arc<RwLock<RobotState>>, mut telem: Receiver<Sensor
 
     }
 }
-
-trait Brownout {
-    /// limit exertion to factor of full output (0 to 1)
-    fn brownout(self, factor: f32) -> Self;
-}
-
-impl Brownout for Command {
-    fn brownout(self, factor: f32) -> Self {
-        // nothing is preserved
-        match self {
-            Self::Twist(fwd, rot) => Self::Twist(
-                fwd.clamp(-factor, factor), 
-                rot.clamp(-factor, factor)
-            ),
-            Self::SetLed(rgb) => {
-                Self::SetLed(rgb.simd_min(Vector3::repeat((factor * u8::MAX as f32) as u8)))
-            }
-            other => other,
-        }
-    }
-}
-
-

southbridge/src/main.rs

@@ -1,12 +1,12 @@
 #![no_std]
 #![no_main]
 
-use core::{panic::PanicInfo, sync::atomic::Ordering};
+use core::{panic::PanicInfo, sync::atomic::{AtomicU16, Ordering}};
 
-use common::{Command, Response, SensorData, BAUDRATE};
+use common::{Response, RobotState, SensorData, BAUDRATE};
 use embassy_executor::Spawner;
 use embassy_rp::{adc::{self, Adc}, bind_interrupts, block::ImageDef, gpio::{Level, Output, Pull}, peripherals::{ADC, ADC_TEMP_SENSOR, PIN_28, UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, BufferedUartTx, Config}, usb::Driver};
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock, watch::Watch};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
 use embedded_io_async::{BufRead, Read, Write};
 use log::{error, info, trace, warn};
@@ -21,9 +21,17 @@ bind_interrupts!(struct Irqs {
     ADC_IRQ_FIFO => adc::InterruptHandler;
 });
 
-pub static COMMANDS: Channel<CriticalSectionRawMutex, Command, 5> = Channel::new();
+pub static COMMANDS: Watch<CriticalSectionRawMutex, (RobotState, Instant), 5> = Watch::new();
 pub static SENSOR_DATA: Channel<CriticalSectionRawMutex, SensorData, 5> = Channel::new();
 
+pub static BUS_VOLTAGE: AtomicU16 = AtomicU16::new(u16::MAX);
+const BUS_ADC_TO_VOLTS: f32 = 251.6763848397;
+
+/// volts
+fn bus_voltage() -> f32 {
+    BUS_VOLTAGE.load(Ordering::Acquire) as f32 / BUS_ADC_TO_VOLTS
+}
+
 #[link_section = ".start_block"]
 #[used]
 pub static IMAGE_DEF: ImageDef = ImageDef::secure_exe();
@@ -49,6 +57,7 @@ async fn main(spawner: Spawner) {
 
     let driver = Driver::new(p.USB, Irqs);
     spawner.spawn(logger_task(driver)).unwrap();
+    info!("logger enabled");
 
     let mut drive_conf: pwm::Config = Default::default();
     drive_conf.divider = 150.into();
@@ -59,6 +68,7 @@ async fn main(spawner: Spawner) {
     let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, stopped.clone());
 
     let disabled_orange = Vector3::new(254, 100, 0);
+    let enabled_green = Vector3::new(0, 140, 0);
 
     let mut light_conf_rg: pwm::Config = Default::default();
     light_conf_rg.divider = 1.into();
@@ -81,9 +91,9 @@ async fn main(spawner: Spawner) {
     let config = embassy_rp::i2c::Config::default();
     let bus = embassy_rp::i2c::I2c::new_async(p.I2C1, p.PIN_19, p.PIN_18, Irqs, config);
 
-    static TX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]);
+    static TX_BUF: ConstStaticCell<[u8; 2048]> = ConstStaticCell::new([0u8;2048]);
     let tx_buf = TX_BUF.take();
-    static RX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]);
+    static RX_BUF: ConstStaticCell<[u8; 2048]> = ConstStaticCell::new([0u8;2048]);
     let rx_buf = RX_BUF.take();
 
     let mut uart_config = Config::default();
@@ -94,63 +104,46 @@ async fn main(spawner: Spawner) {
 
     spawner.spawn(decoder(rx)).unwrap();
 
-    static STARTUP: OnceLock<()> = OnceLock::new();
-
     static ENABLED: AtomicBool = AtomicBool::new(false);
-    spawner.spawn(telemetry(tx, &ENABLED, &STARTUP)).unwrap();
+    spawner.spawn(telemetry(tx, &ENABLED)).unwrap();
 
-    let enabled = &ENABLED;
+    let watchdog_time = Duration::from_millis(150);
-    let mut enable_watchdog = Instant::now();
-    let enable_watchdog_time = Duration::from_millis(150);
 
     info!("ready");
 
+    let mut commands = COMMANDS.receiver().unwrap();
+
     loop {
-        let command = if enabled.load(Ordering::Acquire) { 
+        let motor_update = Timer::after_millis(3);
-            let Ok(command) = with_deadline(enable_watchdog + enable_watchdog_time, COMMANDS.receive()).await else {
+        let (command, time) = commands.get().await;
-                warn!("no message received");
+
-                enabled.store(false, Ordering::Release);
+        if time.elapsed() > watchdog_time {
-                set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone());
-                continue;
-            };
-            command
-        } else {
-            // stop all motors
             drive_pwm.set_config(&stopped);
-            info!("waiting for command");
+            set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone());
-            COMMANDS.receive().await
+            ENABLED.store(false, Ordering::Acquire);
-        };
+            commands.changed().await;
+            set_underglow(&mut light_pwm_rg, &mut light_pwm_b, enabled_green.clone());
+            ENABLED.store(true, Ordering::Acquire);
+            continue;
+        }
 
-        STARTUP.get_or_init(||());
+        ENABLED.store(true, Ordering::Acquire);
 
-        match command {
-            Command::Twist(forward, right) => {
-                drive_conf.compare_a = calc_speed(-forward - right);
-                drive_conf.compare_b = calc_speed(forward - right);
-                drive_pwm.set_config(&drive_conf);
-            },
-            Command::Stop => { 
-                drive_pwm.set_config(&stopped);
-            },
-            Command::Enable => {
-                enabled.store(true, Ordering::Release);
-                enable_watchdog = Instant::now();
-                set_underglow(&mut light_pwm_rg, &mut light_pwm_b, Vector3::new(0, 154, 0));
-            },
-            Command::Disable => {
-                enabled.store(false, Ordering::Release);
-                set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone());
-                drive_pwm.set_config(&stopped);
-            },
-            Command::FeedWatchdog => {
-                enable_watchdog = Instant::now();
-            }
-            Command::SetLed(value) => {
-                set_underglow(&mut light_pwm_rg, &mut light_pwm_b, value);
-            }
-        };
 
-        info!("Blink");
+        // 100% effort at 9.5v, 0% effort at 7.5v
+        let brownout = ((bus_voltage()-7.5)/2.0).clamp(0., 1.);
+        let command = command.brownout(brownout);
+
+        // drive motors
+        let (forward, right) = command.drive;
+        drive_conf.compare_a = calc_speed(-forward - right);
+        drive_conf.compare_b = calc_speed(forward - right);
+        drive_pwm.set_config(&drive_conf);
+
+        // underglow
+        set_underglow(&mut light_pwm_rg, &mut light_pwm_b, command.underglow);
+
+        motor_update.await;
     }
 }
 
@@ -158,6 +151,7 @@ async fn main(spawner: Spawner) {
 #[embassy_executor::task]
 async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
     info!("Reading...");
+    let sender = COMMANDS.sender();
 
     loop {
         let mut len = [0;4];
@@ -165,30 +159,36 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
         let len = u32::from_be_bytes(len) as usize;
 
         let mut buf = [0;1024];
-        rx.read_exact(&mut buf[..len]).await.unwrap();
+        if let Err(e) = rx.read_exact(&mut buf[..len]).await {
+            trace!("read fail {e:?}");
+        };
 
-        let Ok(data) = postcard::from_bytes::<Command>(&buf[..len]) else {
+        let Ok(data) = postcard::from_bytes::<RobotState>(&buf[..len]) else {
             error!("message decode fail");
             continue;
         };
 
         trace!("received {data:?}");
 
-        COMMANDS.send(data).await;
+        sender.send((data, Instant::now()));
     }
 }
 
 /// Receive data from channel and send it over UART
 #[embassy_executor::task]
-async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool, startup: &'static OnceLock<()>) {
+async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool) {
     info!("Telemetry waiting...");
-    startup.get().await;
+    //COMMANDS.receiver().unwrap().get().await;
     info!("Telemetry started...");
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();
 
+        if !enabled.load(Ordering::Release) {
+            Timer::after_millis(10).await;
+            continue;
+        }
+
         let packet = Response {
-            enabled: enabled.load(Ordering::Acquire),
             sensor_data: data,
             uptime_micros: Instant::now().as_micros(),
         };
@@ -201,7 +201,9 @@ async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static Ato
         if let Err(e) = tx.write_all(&serialized).await {
             error!("transport error {e:?}");
         }
-        tx.flush().await.unwrap();
+        if let Err(e) = tx.flush().await {
+            error!("flush err {e:?}");
+        }
     }
 }
 
@@ -231,8 +233,9 @@ async fn bus_voltage_monitor(adc: ADC, bus: PIN_28, temp: ADC_TEMP_SENSOR) {
 
     loop {
         let level = adc.read(&mut bus_voltage).await.unwrap();
+        BUS_VOLTAGE.store(level, Ordering::Release);
         // empirically calculated against $20 microcenter voltmeter (10k & 33k divider circuit)
-        SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / 251.6763848397)).await;
+        SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / BUS_ADC_TO_VOLTS)).await;
         let temp = adc.read(&mut ts).await.unwrap();
         SENSOR_DATA.send(SensorData::AmbientTemperature(convert_to_celsius(temp))).await;
         Timer::after_millis(3).await;