two types of command at the same time (sputters, lags, then dies)

interface/src/main.rs

@@ -14,10 +14,12 @@ async fn main() -> anyhow::Result<()> {
     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")?;
-    let lb = gamepad.button_code(gilrs::Button::LeftTrigger2).context("no lb")?;
+    let lb = gamepad.button_code(gilrs::Button::LeftTrigger).context("no lb")?;
     let a = gamepad.button_code(gilrs::Button::South).context("no a")?;
+    let mut last_a = false;
 
-    let mut robot = TcpStream::connect("pelican.dyn.wpi.edu:3322").await?;
+    let server = "130.215.211.79:3322";
+    let mut robot = TcpStream::connect(server).await?;
     robot.set_nodelay(true)?;
 
     let (telem, robot_controller) = robot.into_split();
@@ -66,16 +68,27 @@ async fn main() -> anyhow::Result<()> {
         }
 
         let cmd = Command::Twist(ly, rx);
+        println!("twist");
 
         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("pelican.dyn.wpi.edu:3322").await?;
+            robot = TcpStream::connect(server).await?;
             robot.set_nodelay(true)?;
 
             let (telem, robot) = robot.into_split();
@@ -85,16 +98,7 @@ async fn main() -> anyhow::Result<()> {
             robot_controller = BufWriter::new(robot);
         };
 
-        let black = Vector3::repeat(0u8);
+        sleep(Duration::from_millis(65)).await;
-        let green = Vector3::new(100u8, 0, 0);
-        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?;
-        sleep(Duration::from_micros(350)).await;
-        robot_controller.flush().await?;
-
-        sleep(Duration::from_micros(3500)).await;
     }
 }
 

northbridge/Cargo.lock

@@ -146,6 +146,62 @@ version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b"
 
+[[package]]
+name = "crossbeam"
+version = "0.8.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1137cd7e7fc0fb5d3c5a8678be38ec56e819125d8d7907411fe24ccb943faca8"
+dependencies = [
+ "crossbeam-channel",
+ "crossbeam-deque",
+ "crossbeam-epoch",
+ "crossbeam-queue",
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-channel"
+version = "0.5.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "06ba6d68e24814cb8de6bb986db8222d3a027d15872cabc0d18817bc3c0e4471"
+dependencies = [
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-deque"
+version = "0.8.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51"
+dependencies = [
+ "crossbeam-epoch",
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-epoch"
+version = "0.9.18"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e"
+dependencies = [
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-queue"
+version = "0.3.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0f58bbc28f91df819d0aa2a2c00cd19754769c2fad90579b3592b1c9ba7a3115"
+dependencies = [
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-utils"
+version = "0.8.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28"
+
 [[package]]
 name = "embedded-io"
 version = "0.4.0"
@@ -443,6 +499,7 @@ version = "0.1.0"
 dependencies = [
  "anyhow",
  "common",
+ "crossbeam",
  "framed",
  "futures",
  "futures-core",

northbridge/Cargo.toml

@@ -15,3 +15,4 @@ nalgebra = { version = "0.31.2", default-features=false, features = ["serde-seri
 tokio-util = { version = "0.7.13", features = ["codec"] }
 futures-core = "0.3.31"
 futures = "0.3.31"
+crossbeam = "0.8.4"

northbridge/justfile

@@ -1,4 +1,6 @@
+server  := 'pelican.dyn.wpi.edu'
+
 deploy:
     cross build --target=armv7-unknown-linux-gnueabihf --release
-    scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@pelican.dyn.wpi.edu:
+    scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@{{server}}:
-    ssh cruise@pelican.dyn.wpi.edu systemctl --user restart nightstand
+    ssh cruise@{{server}} systemctl --user restart nightstand

northbridge/src/framed_codec.rs

@@ -36,7 +36,9 @@ impl Decoder for FramedCodec {
         let remainder = remainder.to_vec();
 
         src.clear();
-        src.extend_from_slice(&remainder);
+        if remainder.len() < 200 { // magic number
+            src.extend_from_slice(&remainder);
+        }
 
         Ok(Some(message))
     }

northbridge/src/main.rs

@@ -4,10 +4,11 @@ use std::{sync::Arc, time::Duration};
 
 use anyhow::{Context, Result};
 use common::{Command, Response, SensorData, BAUDRATE};
+use crossbeam::queue::ArrayQueue;
 use framed_codec::FramedCodec;
-use futures::{SinkExt, StreamExt};
+use futures::{SinkExt, StreamExt, TryStreamExt};
 use nalgebra::{SimdPartialOrd, SimdValue, Vector3};
-use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver}, watch::{self, Sender}, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}};
+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_serial::SerialPortBuilderExt;
 use tokio_util::codec::Framed;
 
@@ -28,7 +29,7 @@ async fn main() -> Result<()> {
 
     let (mut write, mut read) = Framed::new(serial, FramedCodec::new()).split();
 
-    let (send, commands) = watch::channel(Command::Stop);
+    let commands = Arc::new(ArrayQueue::new(5));
 
     let state = Arc::new(RwLock::new(RobotState {
         bus_voltage: f32::MAX,
@@ -38,10 +39,11 @@ 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 _ = send.send(Command::Stop);
+            let _ = sender.force_push(Command::Stop);
-            if let Err(e) = control(send.clone(), control_telem.resubscribe()).await {
+            if let Err(e) = control(sender.clone(), control_telem.resubscribe()).await {
                 println!("controller exited: {e}");
             }
         }
@@ -58,7 +60,6 @@ async fn main() -> Result<()> {
         };
         if data.enabled {
             write.send(Command::FeedWatchdog).await?;
-            write.flush().await?;
 
         } else {
             println!("enabled southbridge");
@@ -73,13 +74,22 @@ async fn main() -> Result<()> {
 
         // 100% effort at 9.5v, 0% effort at 7.5v
         let brownout = ((voltage-7.5)/2.0).clamp(0., 1.);
-        
+
-        write.send(commands.borrow().clone().brownout(brownout)).await?;
+        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: Sender<Command>, mut telem: Receiver<SensorData>) -> Result<()> {
+async fn control(sender: Arc<ArrayQueue<Command>>, mut telem: Receiver<SensorData>) -> Result<()> {
     let listener = TcpListener::bind("0.0.0.0:3322").await?;
 
     let (stream, addr) = listener.accept().await?;
@@ -106,7 +116,8 @@ async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> Re
             match timeout(Duration::from_millis(150), read.read_u32()).await {
                 Ok(it) => { break it?; },
                 Err(Elapsed) => {
-                    sender.send(Command::Stop)?;
+                    sender.force_push(Command::Stop);
+                    continue;
                 },
             };
         };
@@ -115,7 +126,7 @@ async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> Re
 
         let cmd: Command = postcard::from_bytes(&buf)?;
 
-        sender.send(cmd)?;
+        sender.force_push(cmd);
     }
 }