propagate voltage to controller

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}");
+            }
+            _ => {}
+        }
+
+    }
+}

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

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<Result<()>> = 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<Command>) -> Result<()> {
+async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> 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<Result<()>> = 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(100), read.read_u32()).await??;
-        let len = timeout(Duration::from_millis(30), stream.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)?;