thingy drives

added deadzone to gamepad controller

mended drive polarity

wait to start telem until awakened by northbridge
(linux doesn't boot in 300 cycles)

encoder/Cross.toml

@@ -0,0 +1,2 @@
+[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, Gilrs};
+use gilrs::{Axis, Event, Gilrs};
 use tokio::{io::{AsyncWriteExt, BufWriter}, net::TcpStream, time::sleep};
 
 #[tokio::main]
 async fn main() -> anyhow::Result<()> {
     println!("Hello, world!");
 
-    let gamepads = Gilrs::new().unwrap();
+    let mut 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,11 +22,32 @@ async fn main() -> anyhow::Result<()> {
 
     let mut robot_controller = BufWriter::new(robot_controller);
 
-    loop {
-        let values = gamepad.state();
+    let mut active_gamepad = None;
 
-        let ly = values.axis_data(ly).context("no left")?.value();
-        let rx = values.axis_data(rx).context("no left")?.value();
+    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 mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);
+
+        for axis in [&mut rx, &mut ly] {
+            if axis.abs() < 0.05 {
+                *axis = 0.0;
+            }
+        }
 
         let cmd = Command::Twist(ly, rx);
 
@@ -36,7 +57,6 @@ 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 
-    scp target/armv7-unknown-linux-gnueabihf/debug/northbridge cruise@pelican.dyn.wpi.edu:
+    cross build --target=armv7-unknown-linux-gnueabihf --release
+    scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@pelican.dyn.wpi.edu:

northbridge/src/framed_codec.rs

@@ -1,4 +1,4 @@
-use anyhow::anyhow;
+use anyhow::{anyhow, Ok};
 use common::Command;
 use framed::{BoxPayload, FRAME_END_SYMBOL};
 use tokio_util::{bytes::BytesMut, codec::{Decoder, Encoder}};
@@ -15,31 +15,35 @@ impl FramedCodec {
 }
 
 impl Decoder for FramedCodec {
-    type Item = BoxPayload;
-    type Error = framed::Error;
+    type Item = Vec<u8>;
+    type Error = anyhow::Error;
 
     fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
-
-        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)
-            }
+        if src.len() < 4 {
+            return 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 = framed::Error;
+    type Error = anyhow::Error;
 
     fn encode(&mut self, item: Vec<u8>, dst: &mut BytesMut) -> Result<(), Self::Error> {
-        let encoded = self.codec.encode_to_box(&item)?;
-
-        dst.extend_from_slice(&encoded);
+        dst.extend_from_slice(&item.len().to_be_bytes());
+        dst.extend_from_slice(&item);
 
         Ok(())
     }

northbridge/src/main.rs

@@ -30,26 +30,33 @@ async fn main() -> Result<()> {
 
     println!("starting");
     write.send(Command::Enable).await?;
+    write.flush().await?;
 
-    while let Some(Ok(data)) = read.next().await {
+    loop {
+        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?;
         }
 
-        dbg!(&data);
+        write.send(Command::Enable).await?;
+        write.flush().await?;
+
+        //dbg!(&data);
 
         if let Some(data) = data.sensor_data {
             let _ = sensor_sender.send(data);
         }
-
-        write.send(commands.borrow().clone()).await?;
+        
+        write.send(dbg!(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", "executor-interrupt"] }
+embassy-executor = { version = "0.7", features = ["arch-cortex-m", "executor-thread", "task-arena-size-32768"] }
 embassy-sync = { version = "0.6" }
 embassy-time = { version = "0.4" }
 cortex-m = { version = "0.7.6" }

southbridge/src/main.rs

@@ -6,11 +6,9 @@ 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};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
-use embedded_io::Write;
-use embedded_io_async::{BufRead, Read};
-use framed::{bytes, FRAME_END_SYMBOL};
+use embedded_io_async::{BufRead, Read, Write};
 use log::{error, info, trace, warn};
 use nalgebra::clamp;
 use portable_atomic::AtomicBool;
@@ -52,12 +50,12 @@ async fn main(spawner: Spawner) {
     spawner.spawn(logger_task(driver)).unwrap();
 
     let mut drive_conf: pwm::Config = Default::default();
-    drive_conf.divider = 15.into();
-    drive_conf.top = 50000; // 5ms
-    drive_conf.compare_b = 15000; // 1.5ms
-    drive_conf.compare_a = 15000; // 1.5ms
+    drive_conf.divider = 150.into();
+    drive_conf.top = 20000; // 20ms
+    drive_conf.compare_b = 1500; // 1.5ms
+    drive_conf.compare_a = 1500; // 1.5ms
     let stopped = drive_conf.clone();
-    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, drive_conf.clone());
+    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, stopped.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);
@@ -75,24 +73,18 @@ 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)).unwrap();
+    spawner.spawn(telemetry(tx, &ENABLED, &STARTUP)).unwrap();
 
     let enabled = &ENABLED;
     let mut enable_watchdog = Instant::now();
-    let enable_watchdog_time = Duration::from_millis(50);
+    let enable_watchdog_time = Duration::from_millis(150);
 
-    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");
@@ -101,14 +93,18 @@ 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_b = calc_speed(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 => { 
@@ -129,7 +125,6 @@ async fn main(spawner: Spawner) {
         };
 
         info!("Blink");
-        Timer::after(Duration::from_millis(100)).await;
     }
 }
 
@@ -137,18 +132,14 @@ 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 data = rx.fill_buf().await.unwrap();
-        let Some(end) =  data.iter().position(|e| *e == FRAME_END_SYMBOL) else { continue; };
-        let data = &data[..end];
+        let mut len = [0;4];
+        rx.read_exact(&mut len).await.unwrap();
+        let len = u32::from_be_bytes(len) as usize;
 
-        let mut buf = [0u8; 1024];
-        let Ok(len) = codec.decode_to_slice(data, &mut buf) else {
-            error!("frame decode fail");
-            continue;
-        };
+        let mut buf = [0;1024];
+        rx.read_exact(&mut buf[..len]).await.unwrap();
 
         let Ok(data) = postcard::from_bytes::<Command>(&buf[..len]) else {
             error!("message decode fail");
@@ -158,17 +149,15 @@ 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) {
-    info!("Reading...");
-    let mut codec = framed::bytes::Config::default().to_codec();
-
+async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool, startup: &'static OnceLock<()>) {
+    info!("Telemetry waiting...");
+    startup.get().await;
+    info!("Telemetry started...");
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();
 
@@ -183,21 +172,20 @@ async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static Ato
             continue;
         };
 
-        let mut buf = [0u8; 1024];
-        let Ok(len) = codec.encode_to_slice(&serialized, &mut buf) else {
-            error!("encoding error");
-            continue;
-        };
-
-        if let Err(e) = tx.write_all(&buf[..len]) {
+        let len = u32::to_be_bytes(serialized.len() as u32);
+        if let Err(e) = tx.write_all(&len).await {
             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 = 10000.;
+    const COUNTS_PER_MS: f32 = 1000.;
     let speed = speed.clamp(-1., 1.);
 
     let ms = (speed/2.0)+1.5;