From b131ad895792dd4182dd8f248f938d30fd40d316 Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Thu, 13 Feb 2025 15:57:34 -0500 Subject: [PATCH] two types of command at the same time (sputters, lags, then dies) --- interface/src/main.rs | 30 +++++++++-------- northbridge/Cargo.lock | 57 +++++++++++++++++++++++++++++++++ northbridge/Cargo.toml | 1 + northbridge/justfile | 6 ++-- northbridge/src/framed_codec.rs | 4 ++- northbridge/src/main.rs | 33 ++++++++++++------- 6 files changed, 104 insertions(+), 27 deletions(-) diff --git a/interface/src/main.rs b/interface/src/main.rs index eec7eed..7304de9 100644 --- a/interface/src/main.rs +++ b/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); - 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; + sleep(Duration::from_millis(65)).await; } } diff --git a/northbridge/Cargo.lock b/northbridge/Cargo.lock index 49842eb..3be6b33 100644 --- a/northbridge/Cargo.lock +++ b/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", diff --git a/northbridge/Cargo.toml b/northbridge/Cargo.toml index 45ca935..e8095af 100644 --- a/northbridge/Cargo.toml +++ b/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" diff --git a/northbridge/justfile b/northbridge/justfile index 607ea10..8ea5b3a 100644 --- a/northbridge/justfile +++ b/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: - ssh cruise@pelican.dyn.wpi.edu systemctl --user restart nightstand + scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@{{server}}: + ssh cruise@{{server}} systemctl --user restart nightstand diff --git a/northbridge/src/framed_codec.rs b/northbridge/src/framed_codec.rs index 456019a..5b0312b 100644 --- a/northbridge/src/framed_codec.rs +++ b/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)) } diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs index 357b8d4..4c3a791 100644 --- a/northbridge/src/main.rs +++ b/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); - if let Err(e) = control(send.clone(), control_telem.resubscribe()).await { + let _ = sender.force_push(Command::Stop); + 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, mut telem: Receiver) -> Result<()> { +async fn control(sender: Arc>, mut telem: Receiver) -> 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, mut telem: Receiver) -> 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, mut telem: Receiver) -> Re let cmd: Command = postcard::from_bytes(&buf)?; - sender.send(cmd)?; + sender.force_push(cmd); } }