encoder/Cross.toml

@@ -1,2 +0,0 @@
-[target.armv7-unknown-linux-gnueabihf]
-image = "ghcr.io/cross-rs/armv7-unknown-linux-gnueabihf:main"

interface/src/main.rs

@@ -2,14 +2,14 @@ use std::time::Duration;
 
 use anyhow::Context;
 use common::Command;
-use gilrs::{Axis, Event, Gilrs};
+use gilrs::{Axis, Gilrs};
 use tokio::{io::{AsyncWriteExt, BufWriter}, net::TcpStream, time::sleep};
 
 #[tokio::main]
 async fn main() -> anyhow::Result<()> {
     println!("Hello, world!");
 
-    let mut gamepads = Gilrs::new().unwrap();
+    let gamepads = Gilrs::new().unwrap();
     let (_, gamepad) = gamepads.gamepads().nth(0).context("no gamepads")?;
     let ly = gamepad.axis_code(Axis::LeftStickY).context("no left joystick")?;
     let rx = gamepad.axis_code(Axis::RightStickX).context("no right joystick")?;
@@ -22,32 +22,11 @@ async fn main() -> anyhow::Result<()> {
 
     let mut robot_controller = BufWriter::new(robot_controller);
 
-    let mut active_gamepad = None;
-
     loop {
-        while let Some(Event { id,..}) = gamepads.next_event() {
-            active_gamepad = Some(id);
-        }
-
-        let Some(gamepad) = active_gamepad else {
-            continue;
-        };
-
-        let gamepad = gamepads.gamepad(gamepad);
-
-
-
         let values = gamepad.state();
-        dbg!(values);
 
-        let mut ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0);
+        let ly = values.axis_data(ly).context("no left")?.value();
-        let mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);
+        let rx = values.axis_data(rx).context("no left")?.value();
-
-        for axis in [&mut rx, &mut ly] {
-            if axis.abs() < 0.05 {
-                *axis = 0.0;
-            }
-        }
 
         let cmd = Command::Twist(ly, rx);
 
@@ -57,6 +36,7 @@ async fn main() -> anyhow::Result<()> {
         robot_controller.write_all(&encoded).await?;
 
         robot_controller.flush().await?;
+
         sleep(Duration::from_micros(3500)).await;
     }
 }

northbridge/justfile

@@ -1,3 +1,3 @@
 deploy:
-    cross build --target=armv7-unknown-linux-gnueabihf --release
+    cross build --target=armv7-unknown-linux-gnueabihf 
-    scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@pelican.dyn.wpi.edu:
+    scp target/armv7-unknown-linux-gnueabihf/debug/northbridge cruise@pelican.dyn.wpi.edu:

northbridge/src/framed_codec.rs

@@ -1,4 +1,4 @@
-use anyhow::{anyhow, Ok};
+use anyhow::anyhow;
 use common::Command;
 use framed::{BoxPayload, FRAME_END_SYMBOL};
 use tokio_util::{bytes::BytesMut, codec::{Decoder, Encoder}};
@@ -15,35 +15,31 @@ impl FramedCodec {
 }
 
 impl Decoder for FramedCodec {
-    type Item = Vec<u8>;
+    type Item = BoxPayload;
-    type Error = anyhow::Error;
+    type Error = framed::Error;
 
     fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
-        if src.len() < 4 {
+
-            return Ok(None);
+        match src.iter().position(|e| *e==FRAME_END_SYMBOL) {
+            Some(len) => {
+                let data = src.split_to(len);
+
+                Ok(Some(self.codec.decode_to_box(&data)?))
+            }
+            None => {
+                Ok(None)
             }
-
-        let len = u32::from_be_bytes(src[..4].try_into()?) as usize;
-
-        if src.len() < len + 4 {
-            return Ok(None);
         }
-
-        let data = src.split_to(len + 4);
-
-        let mut out = Vec::new();
-        out.extend_from_slice(&data[4..]);
-
-        Ok(Some(out))
     }
 }
 
 impl Encoder<Vec<u8>> for FramedCodec {
-    type Error = anyhow::Error;
+    type Error = framed::Error;
 
     fn encode(&mut self, item: Vec<u8>, dst: &mut BytesMut) -> Result<(), Self::Error> {
-        dst.extend_from_slice(&item.len().to_be_bytes());
+        let encoded = self.codec.encode_to_box(&item)?;
-        dst.extend_from_slice(&item);
+
+        dst.extend_from_slice(&encoded);
 
         Ok(())
     }

northbridge/src/main.rs

@@ -30,33 +30,26 @@ async fn main() -> Result<()> {
 
     println!("starting");
     write.send(Command::Enable).await?;
-    write.flush().await?;
 
-    loop {
+    while let Some(Ok(data)) = read.next().await {
-        let Some(Ok(data)) = read.next().await else {
-            continue;
-        };
         if data.enabled {
             write.send(Command::FeedWatchdog).await?;
-            write.flush().await?;
 
         } else {
             println!("enabled southbridge");
             write.send(Command::Enable).await?;
         }
 
-        write.send(Command::Enable).await?;
+        dbg!(&data);
-        write.flush().await?;
-
-        //dbg!(&data);
 
         if let Some(data) = data.sensor_data {
             let _ = sensor_sender.send(data);
         }
 
-        write.send(dbg!(commands.borrow().clone())).await?;
+        write.send(commands.borrow().clone()).await?;
-        write.flush().await?;
     }
+
+    Ok(())
 }
 
 async fn control(sender: Sender<Command>) -> Result<()> {

southbridge/Cargo.toml

@@ -13,7 +13,7 @@ embedded-storage = "0.3.1"
 
 cortex-m-rt = "0.7.3"
 
-embassy-executor = { version = "0.7", features = ["arch-cortex-m", "executor-thread", "task-arena-size-32768"] }
+embassy-executor = { version = "0.7", features = ["arch-cortex-m", "executor-thread", "executor-interrupt"] }
 embassy-sync = { version = "0.6" }
 embassy-time = { version = "0.4" }
 cortex-m = { version = "0.7.6" }

southbridge/src/main.rs

@@ -6,9 +6,11 @@ use core::{panic::PanicInfo, sync::atomic::Ordering};
 use common::{Command, Response, SensorData, BAUDRATE};
 use embassy_executor::Spawner;
 use embassy_rp::{bind_interrupts, block::ImageDef, gpio::{Level, Output}, peripherals::{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};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
-use embedded_io_async::{BufRead, Read, Write};
+use embedded_io::Write;
+use embedded_io_async::{BufRead, Read};
+use framed::{bytes, FRAME_END_SYMBOL};
 use log::{error, info, trace, warn};
 use nalgebra::clamp;
 use portable_atomic::AtomicBool;
@@ -50,12 +52,12 @@ async fn main(spawner: Spawner) {
     spawner.spawn(logger_task(driver)).unwrap();
 
     let mut drive_conf: pwm::Config = Default::default();
-    drive_conf.divider = 150.into();
+    drive_conf.divider = 15.into();
-    drive_conf.top = 20000; // 20ms
+    drive_conf.top = 50000; // 5ms
-    drive_conf.compare_b = 1500; // 1.5ms
+    drive_conf.compare_b = 15000; // 1.5ms
-    drive_conf.compare_a = 1500; // 1.5ms
+    drive_conf.compare_a = 15000; // 1.5ms
     let stopped = drive_conf.clone();
-    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, stopped.clone());
+    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, drive_conf.clone());
 
     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);
@@ -73,18 +75,24 @@ 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 mut enable_watchdog = Instant::now();
-    let enable_watchdog_time = Duration::from_millis(150);
+    let enable_watchdog_time = Duration::from_millis(50);
 
+    Timer::after_secs(1).await;
     info!("ready");
+    Timer::after_secs(1).await;
+
 
     loop {
+        if !enabled.load(Ordering::Acquire) {
+            // stop all motors
+            drive_pwm.set_config(&stopped);
+        }
+
         let command = if enabled.load(Ordering::Acquire) { 
             let Ok(command) = with_deadline(enable_watchdog + enable_watchdog_time, COMMANDS.receive()).await else {
                 warn!("no message received");
@@ -93,18 +101,14 @@ async fn main(spawner: Spawner) {
             };
             command
         } else {
-            // stop all motors
-            //drive_pwm.set_config(&stopped);
             info!("waiting for command");
             COMMANDS.receive().await
         };
 
-        STARTUP.get_or_init(||());
-
         match command {
             Command::Twist(forward, right) => {
-                drive_conf.compare_a = calc_speed(-forward - right);
+                drive_conf.compare_a = calc_speed(forward - right);
-                drive_conf.compare_b = calc_speed(forward - right);
+                drive_conf.compare_b = calc_speed(forward + right);
                 drive_pwm.set_config(&drive_conf);
             },
             Command::Stop => { 
@@ -125,6 +129,7 @@ async fn main(spawner: Spawner) {
         };
 
         info!("Blink");
+        Timer::after(Duration::from_millis(100)).await;
     }
 }
 
@@ -132,14 +137,18 @@ async fn main(spawner: Spawner) {
 #[embassy_executor::task]
 async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
     info!("Reading...");
+    let mut codec = framed::bytes::Config::default().to_codec();
 
     loop {
-        let mut len = [0;4];
+        let data = rx.fill_buf().await.unwrap();
-        rx.read_exact(&mut len).await.unwrap();
+        let Some(end) =  data.iter().position(|e| *e == FRAME_END_SYMBOL) else { continue; };
-        let len = u32::from_be_bytes(len) as usize;
+        let data = &data[..end];
 
-        let mut buf = [0;1024];
+        let mut buf = [0u8; 1024];
-        rx.read_exact(&mut buf[..len]).await.unwrap();
+        let Ok(len) = codec.decode_to_slice(data, &mut buf) else {
+            error!("frame decode fail");
+            continue;
+        };
 
         let Ok(data) = postcard::from_bytes::<Command>(&buf[..len]) else {
             error!("message decode fail");
@@ -149,15 +158,17 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
         trace!("received {data:?}");
 
         COMMANDS.send(data).await;
+
+        rx.consume(end);
     }
 }
 
 /// 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...");
+    info!("Reading...");
-    startup.get().await;
+    let mut codec = framed::bytes::Config::default().to_codec();
-    info!("Telemetry started...");
+
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();
 
@@ -172,20 +183,21 @@ async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static Ato
             continue;
         };
 
-        let len = u32::to_be_bytes(serialized.len() as u32);
+        let mut buf = [0u8; 1024];
-        if let Err(e) = tx.write_all(&len).await {
+        let Ok(len) = codec.encode_to_slice(&serialized, &mut buf) else {
+            error!("encoding error");
+            continue;
+        };
+
+        if let Err(e) = tx.write_all(&buf[..len]) {
             error!("transport error {e:?}");
         }
-        if let Err(e) = tx.write_all(&serialized).await {
-            error!("transport error {e:?}");
-        }
-        tx.flush().await.unwrap();
     }
 }
 
 /// -1 to 1
 fn calc_speed(speed: f32) -> u16 {
-    const COUNTS_PER_MS: f32 = 1000.;
+    const COUNTS_PER_MS: f32 = 10000.;
     let speed = speed.clamp(-1., 1.);
 
     let ms = (speed/2.0)+1.5;