diff --git a/interface/src/main.rs b/interface/src/main.rs index b8b3095..1351945 100644 --- a/interface/src/main.rs +++ b/interface/src/main.rs @@ -1,9 +1,10 @@ use std::time::Duration; use anyhow::Context; -use common::Command; +use common::{Command, SensorData}; +use futures::Stream; use gilrs::{Axis, Event, Gilrs}; -use tokio::{io::{AsyncWriteExt, BufWriter}, net::TcpStream, time::sleep}; +use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}}; #[tokio::main] async fn main() -> anyhow::Result<()> { @@ -17,16 +18,19 @@ async fn main() -> anyhow::Result<()> { let mut robot = TcpStream::connect("pelican.dyn.wpi.edu:3322").await?; robot.set_nodelay(true)?; + let (telem, robot_controller) = robot.into_split(); - let (_telem, robot_controller) = robot.split(); + tokio::spawn(print_telem(telem)); let mut robot_controller = BufWriter::new(robot_controller); let mut active_gamepad = None; + let mut last_event = Instant::now(); loop { while let Some(Event { id,..}) = gamepads.next_event() { active_gamepad = Some(id); + last_event = Instant::now(); } let Some(gamepad) = active_gamepad else { @@ -49,6 +53,11 @@ async fn main() -> anyhow::Result<()> { } } + if !gamepad.is_connected() { + ly = 0.0; + rx = 0.0; + } + let cmd = Command::Twist(ly, rx); let encoded = postcard::to_stdvec(&cmd)?; @@ -56,7 +65,34 @@ async fn main() -> anyhow::Result<()> { robot_controller.write_u32(encoded.len() as u32).await?; robot_controller.write_all(&encoded).await?; - robot_controller.flush().await?; + if let Err(e) = robot_controller.flush().await { + println!("flush fail: {e}, reconnecting"); + robot = TcpStream::connect("pelican.dyn.wpi.edu:3322").await?; + robot.set_nodelay(true)?; + + let (telem, robot) = robot.into_split(); + + tokio::spawn(print_telem(telem)); + + robot_controller = BufWriter::new(robot); + }; sleep(Duration::from_micros(3500)).await; } } + +async fn print_telem(mut telem: OwnedReadHalf) -> anyhow::Result<()> { + loop { + let len = telem.read_u32().await? as usize; + let mut buf = vec![0; len]; + telem.read_exact(&mut buf).await?; + let telem: SensorData = postcard::from_bytes(&buf)?; + + match telem { + SensorData::BusVoltage(voltage) => { + println!("voltage: {voltage}"); + } + _ => {} + } + + } +} diff --git a/northbridge/justfile b/northbridge/justfile index e3c087f..607ea10 100644 --- a/northbridge/justfile +++ b/northbridge/justfile @@ -1,3 +1,4 @@ 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 diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs index 321903d..c49f191 100644 --- a/northbridge/src/main.rs +++ b/northbridge/src/main.rs @@ -3,10 +3,10 @@ use std::time::Duration; use anyhow::{Context, Result}; -use common::{Command, Response, BAUDRATE}; +use common::{Command, Response, SensorData, BAUDRATE}; use framed_codec::FramedCodec; use futures::{SinkExt, StreamExt}; -use tokio::{io::AsyncReadExt, net::{TcpListener, TcpSocket}, sync::{broadcast, watch::{self, Sender}}, task::JoinHandle, time::timeout}; +use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, Receiver}, watch::{self, Sender}}, task::JoinHandle, time::timeout}; use tokio_serial::SerialPortBuilderExt; use tokio_util::codec::Framed; @@ -16,7 +16,7 @@ mod framed_codec; async fn main() -> Result<()> { let mut serial = tokio_serial::new("/dev/ttyAMA0", BAUDRATE).open_native_async()?; - let (sensor_sender, mut sensor_data) = broadcast::channel(5); + let (sensor_sender, sensor_data) = broadcast::channel(5); serial.set_exclusive(false)?; @@ -24,16 +24,18 @@ async fn main() -> Result<()> { let (send, commands) = watch::channel(Command::Stop); + let mut print_telem = sensor_data.resubscribe(); let _: JoinHandle> = tokio::spawn(async move { loop { - println!("sensor {:?}", sensor_data.recv().await?); + println!("sensor {:?}", print_telem.recv().await?); } }); + let control_telem = sensor_data.resubscribe(); tokio::spawn(async move { loop { let _ = send.send(Command::Stop); - if let Err(e) = control(send.clone()).await { + if let Err(e) = control(send.clone(), control_telem.resubscribe()).await { println!("controller exited: {e}"); } } @@ -70,18 +72,28 @@ async fn main() -> Result<()> { } } -async fn control(sender: Sender) -> Result<()> { +async fn control(sender: Sender, mut telem: Receiver) -> Result<()> { let listener = TcpListener::bind("0.0.0.0:3322").await?; - let (mut stream, addr) = listener.accept().await?; + let (stream, addr) = listener.accept().await?; println!("connected to {addr:?}"); + let (mut read, mut write) = stream.into_split(); + + let _: JoinHandle> = tokio::spawn(async move { + loop { + let data = telem.recv().await?; + let data = postcard::to_stdvec(&data)?; + write.write_u32(data.len() as u32).await?; + write.write_all(&data).await?; + } + }); + loop { - - let len = timeout(Duration::from_millis(30), stream.read_u32()).await??; + let len = timeout(Duration::from_millis(100), read.read_u32()).await??; let mut buf = vec![0; len as usize]; - timeout(Duration::from_millis(30), stream.read_exact(&mut buf)).await??; + timeout(Duration::from_millis(100), read.read_exact(&mut buf)).await??; let cmd: Command = postcard::from_bytes(&buf)?;