From ee67db98ab9219f298dfc99a9107a73a3cd518ee Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Sat, 19 Jul 2025 17:44:13 -0500
Subject: [PATCH] receive telemetry on a separate thread

---
 converter/src/main.rs        | 24 ++++++++++++------------
 robot-controller/src/main.rs | 27 +++++++++++++++------------
 2 files changed, 27 insertions(+), 24 deletions(-)

diff --git a/converter/src/main.rs b/converter/src/main.rs
index 8a8e2d0..f95110c 100644
--- a/converter/src/main.rs
+++ b/converter/src/main.rs
@@ -5,35 +5,35 @@ use postcard::to_vec_cobs;
 use heapless::Vec;
 
 fn main() {
-    let mut port = serialport::new("/dev/ttyACM1", 115200)
+    let mut port = serialport::new("/dev/ttyACM0", 115200)
         .timeout(Duration::from_secs(1000))
         .open().expect("port missing");
     println!("connected over serial");
 
-    let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: false}));
+    let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: true}));
     let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));
 
+    // LabVIEW communication thread
     {
         let command = command.clone();
         let telemetry = telemetry.clone();
         std::thread::spawn(move || server(command,telemetry));
     }
 
-
+    // Telemetry receive thread
+    {
         let reader = port.try_clone().unwrap();
-        let mut reader = BufReader::new(reader);
+        let reader = BufReader::new(reader);
+        let telemetry = telemetry.clone();
+        std::thread::spawn(move || recv_telem(telemetry, reader));
+    }
+
+    // Command transmit thread (main)
     loop {
         let command = command.lock().unwrap().clone();
         let encoded: Vec<u8, 20> = to_vec_cobs(&command).unwrap();
         port.write(&encoded).expect("port write fail");
-        println!("sending");
 
-        let mut recv_buf = std::vec::Vec::new();
-        let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
-        if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
-            println!("dist: {}", telem.distance);
-            *telemetry.lock().unwrap() = telem;
-        }
         sleep(Duration::from_millis(200));
     }
 }
@@ -43,9 +43,9 @@ fn recv_telem<T: Read>(telemetry: Arc<Mutex<Telemetry>>, mut reader: BufReader<T
     loop {
         let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
         if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
-            println!("dist: {}", telem.distance);
             *telemetry.lock().unwrap() = telem;
         }
+        recv_buf.clear();
     }
 }
 
diff --git a/robot-controller/src/main.rs b/robot-controller/src/main.rs
index c33c7bd..12cf054 100644
--- a/robot-controller/src/main.rs
+++ b/robot-controller/src/main.rs
@@ -206,20 +206,23 @@ fn main() -> ! {
                 telem.distance = f32::NAN;
             }
 
-            let mut buf = [0;20];
-            let encoded = postcard::to_slice_cobs(&telem, &mut buf).unwrap();
-
-            for word in encoded {
-                let _ = tx.write(*word);
-                let _ = tx.flush();
-                arduino_hal::delay_us(100);
-            }
-
-            arduino_hal::delay_ms(250);
         } else {
-            let _ = tx.write(0);
+            // ultrasonic disabled
+            telem.distance = f32::NAN;
+        }
+
+        let mut buf = [0;20];
+        let encoded = postcard::to_slice_cobs(&telem, &mut buf).unwrap();
+
+        for word in encoded {
+            let _ = tx.write(*word);
             let _ = tx.flush();
-            arduino_hal::delay_us(500);
+            arduino_hal::delay_us(100); // delay to ensure linux can keep up
+        }
+
+        if command.ultrasonic_enable {
+            // wait for the echoes to fade
+            arduino_hal::delay_ms(50);
         }
     }
 }