ultrasonic enable toggle

common/src/lib.rs

@@ -8,6 +8,7 @@ pub struct Command {
     pub left: f32,
     /// duty cycle, -1.0 - 1.0
     pub right: f32,
+    pub ultrasonic_enable: bool,
 }
 
 #[derive(Serialize, Deserialize, Default, Clone)]

converter/Cargo.lock

@@ -2,6 +2,12 @@
 # It is not intended for manual editing.
 version = 4
 
+[[package]]
+name = "anyhow"
+version = "1.0.98"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e16d2d3311acee920a9eb8d33b8cbc1787ce4a264e85f964c2404b969bdcd487"
+
 [[package]]
 name = "atomic-polyfill"
 version = "1.0.3"
@@ -61,6 +67,7 @@ dependencies = [
 name = "converter"
 version = "0.1.0"
 dependencies = [
+ "anyhow",
  "common",
  "heapless",
  "postcard",

converter/Cargo.toml

@@ -9,3 +9,4 @@ postcard = "1.1.2"
 common = { path = "../common" }
 heapless = "0.7.17"
 serde_json = "1.0.140"
+anyhow = "1.0.98"

converter/src/main.rs

@@ -5,12 +5,12 @@ use postcard::to_vec_cobs;
 use heapless::Vec;
 
 fn main() {
-    let mut port = serialport::new("/dev/ttyACM0", 115200)
+    let mut port = serialport::new("/dev/ttyACM1", 115200)
-        .timeout(Duration::from_secs(1))
+        .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}));
+    let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: false}));
     let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));
 
     {
@@ -19,10 +19,9 @@ fn main() {
         std::thread::spawn(move || server(command,telemetry));
     }
 
+
         let reader = port.try_clone().unwrap();
-
         let mut reader = BufReader::new(reader);
-
     loop {
         let command = command.lock().unwrap().clone();
         let encoded: Vec<u8, 20> = to_vec_cobs(&command).unwrap();
@@ -35,36 +34,59 @@ fn main() {
             println!("dist: {}", telem.distance);
             *telemetry.lock().unwrap() = telem;
         }
-
         sleep(Duration::from_millis(200));
     }
 }
 
+fn recv_telem<T: Read>(telemetry: Arc<Mutex<Telemetry>>, mut reader: BufReader<T>) {
+    let mut recv_buf = std::vec::Vec::new();
+    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;
+        }
+    }
+}
+
 fn server(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>) {
     let listener = TcpListener::bind("0.0.0.0:4242").unwrap();
 
     while let Ok((stream, _)) = listener.accept() {
         println!("connected over IP");
+        let command1 = command.clone();
+        let telemetry1 = telemetry.clone();
+        std::thread::spawn(move || {
+            if let Err(e) = handle_connection(command1, telemetry1, stream) {
+                println!("IP connection dropped: {}", e);
+            }
+        });
+    }
+}
+
+fn handle_connection(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>, stream: std::net::TcpStream) -> anyhow::Result<()> {
     let writer = stream.try_clone().unwrap();
     let mut writer = BufWriter::new(writer);
     let mut reader = BufReader::new(stream);
 
+    loop {
         let mut data_buf = std::vec::Vec::new();
-        let len = reader.read_until(b'\n', &mut data_buf).unwrap();
+        let len = reader.read_until(b'\n', &mut data_buf)?;
 
-        let Ok(new_command) = serde_json::from_slice(&data_buf[0..len]) else {continue;};
+        let Ok(new_command) = serde_json::from_slice::<Command>(&data_buf[0..len]) else {continue;};
 
-        *command.lock().unwrap() = new_command;
+        *command.lock().unwrap() = new_command.clone();
-        println!("received");
+        println!("received l{} r{}", new_command.left, new_command.right);
 
-        let telem = telemetry.lock().unwrap().clone();
+        let mut telem = telemetry.lock().unwrap().clone();
 
-        if telem.distance == f32::NAN {
+        if telem.distance.is_nan() {
-            continue;
+            telem.distance = -1.0;
         }
+        println!("sent d{}", telem.distance);
 
         serde_json::to_writer(&mut writer, &telem).unwrap();
-        writer.write(&[b'\n']).unwrap();
+        writer.write(&[b'\r',b'\n'])?;
-        writer.flush().unwrap();
+        writer.flush()?;
     }
 }

robot-controller/src/main.rs

@@ -163,6 +163,7 @@ fn main() -> ! {
 
         update_shift_register(&mut data_pin, &mut latch_pin, &mut clock_pin, &shift_register);
 
+        if command.ultrasonic_enable {
             // reset timer
             ultrasonic_timer.tcnt1.write(|w| w.bits(0));
 
@@ -215,6 +216,11 @@ fn main() -> ! {
             }
 
             arduino_hal::delay_ms(250);
+        } else {
+            let _ = tx.write(0);
+            let _ = tx.flush();
+            arduino_hal::delay_us(500);
+        }
     }
 }