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);
 
+        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 writer = stream.try_clone().unwrap();
+        let command1 = command.clone();
-        let mut writer = BufWriter::new(writer);
+        let telemetry1 = telemetry.clone();
-        let mut reader = BufReader::new(stream);
+        std::thread::spawn(move || {
-
+            if let Err(e) = handle_connection(command1, telemetry1, stream) {
-        let mut data_buf = std::vec::Vec::new();
+                println!("IP connection dropped: {}", e);
-        let len = reader.read_until(b'\n', &mut data_buf).unwrap();
+            }
-
+        });
-        let Ok(new_command) = serde_json::from_slice(&data_buf[0..len]) else {continue;};
+    }
-
+}
-        *command.lock().unwrap() = new_command;
+
-        println!("received");
+fn handle_connection(command: Arc<Mutex<Command>>, telemetry: Arc<Mutex<Telemetry>>, stream: std::net::TcpStream) -> anyhow::Result<()> {
-
+    let writer = stream.try_clone().unwrap();
-        let telem = telemetry.lock().unwrap().clone();
+    let mut writer = BufWriter::new(writer);
-
+    let mut reader = BufReader::new(stream);
-        if telem.distance == f32::NAN {
+
-            continue;
+    loop {
-        }
+        let mut data_buf = std::vec::Vec::new();
-
+        let len = reader.read_until(b'\n', &mut data_buf)?;
-        serde_json::to_writer(&mut writer, &telem).unwrap();
+
-        writer.write(&[b'\n']).unwrap();
+        let Ok(new_command) = serde_json::from_slice::<Command>(&data_buf[0..len]) else {continue;};
-        writer.flush().unwrap();
+
+        *command.lock().unwrap() = new_command.clone();
+        println!("received l{} r{}", new_command.left, new_command.right);
+
+        let mut telem = telemetry.lock().unwrap().clone();
+
+        if telem.distance.is_nan() {
+            telem.distance = -1.0;
+        }
+        println!("sent d{}", telem.distance);
+
+        serde_json::to_writer(&mut writer, &telem).unwrap();
+        writer.write(&[b'\r',b'\n'])?;
+        writer.flush()?;
     }
 }

robot-controller/src/main.rs

@@ -163,58 +163,64 @@ fn main() -> ! {
 
         update_shift_register(&mut data_pin, &mut latch_pin, &mut clock_pin, &shift_register);
 
-        // reset timer
+        if command.ultrasonic_enable {
-        ultrasonic_timer.tcnt1.write(|w| w.bits(0));
+            // reset timer
-
-        // send ultrasonic pulse
-        trig.set_high();
-        arduino_hal::delay_us(10);
-        trig.set_low();
-
-        let mut response = true;
-
-        /// 0.1s/4µs = 25,000
-        const SIGNAL_START_TIMEOUT: u16 = 25000;
-
-        // wait for return signal start
-        while echo.is_low() {
-            if ultrasonic_timer.tcnt1.read().bits() >= SIGNAL_START_TIMEOUT {
-                response = false;
-                break;
-            }
-        }
-
-        if response {
-            let remainder = ultrasonic_timer.tcnt1.read().bits();
             ultrasonic_timer.tcnt1.write(|w| w.bits(0));
 
-            // Wait for the echo to get low again
+            // send ultrasonic pulse
-            while echo.is_high() {}
+            trig.set_high();
-            let counts = ultrasonic_timer.tcnt1.read().bits();
+            arduino_hal::delay_us(10);
-            let microseconds = counts.saturating_mul(4);
+            trig.set_low();
 
-            //  inch per 148 microseconds
+            let mut response = true;
-            telem.distance = microseconds as f32 / 148.;
-            if microseconds == u16::MAX { telem.distance = f32::NAN };
 
-            // delay for the rest of the return signal timeout if any is left
+            /// 0.1s/4µs = 25,000
-            ultrasonic_timer.tcnt1.write(|w| w.bits(SIGNAL_START_TIMEOUT.saturating_sub(remainder + counts)));
+            const SIGNAL_START_TIMEOUT: u16 = 25000;
-            while ultrasonic_timer.tcnt1.read().bits() <= SIGNAL_START_TIMEOUT {}
 
+            // wait for return signal start
+            while echo.is_low() {
+                if ultrasonic_timer.tcnt1.read().bits() >= SIGNAL_START_TIMEOUT {
+                    response = false;
+                    break;
+                }
+            }
+
+            if response {
+                let remainder = ultrasonic_timer.tcnt1.read().bits();
+                ultrasonic_timer.tcnt1.write(|w| w.bits(0));
+
+                // Wait for the echo to get low again
+                while echo.is_high() {}
+                let counts = ultrasonic_timer.tcnt1.read().bits();
+                let microseconds = counts.saturating_mul(4);
+
+                //  inch per 148 microseconds
+                telem.distance = microseconds as f32 / 148.;
+                if microseconds == u16::MAX { telem.distance = f32::NAN };
+
+                // delay for the rest of the return signal timeout if any is left
+                ultrasonic_timer.tcnt1.write(|w| w.bits(SIGNAL_START_TIMEOUT.saturating_sub(remainder + counts)));
+                while ultrasonic_timer.tcnt1.read().bits() <= SIGNAL_START_TIMEOUT {}
+
+            } else {
+                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 {
-            telem.distance = f32::NAN;
+            let _ = tx.write(0);
-        }
-
-        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_us(500);
         }
-
-        arduino_hal::delay_ms(250);
     }
 }