return ultrasonic data

common/src/lib.rs

@@ -4,6 +4,14 @@ use serde::Deserialize;
 
 #[derive(Serialize, Deserialize, Default, Clone)]
 pub struct Command {
+    /// duty cycle, -1.0 - 1.0
     pub left: f32,
+    /// duty cycle, -1.0 - 1.0
     pub right: f32,
 }
+
+#[derive(Serialize, Deserialize, Default, Clone)]
+pub struct Telemetry {
+    /// inches, front ultrasonic sensor
+    pub distance: f32,
+}

converter/src/main.rs

@@ -1,35 +1,52 @@
-use std::{io::{BufRead, BufReader}, net::TcpListener, sync::{Arc, Mutex}, thread::sleep, time::Duration};
+use std::{f32::NAN, io::{BufRead, BufReader, BufWriter, Read, Write}, net::TcpListener, sync::{Arc, Mutex}, thread::sleep, time::Duration};
 
-use common::Command;
+use common::{Command, Telemetry};
 use postcard::to_vec_cobs;
 use heapless::Vec;
 
 fn main() {
     let mut port = serialport::new("/dev/ttyACM0", 115200)
+        .timeout(Duration::from_secs(1))
         .open().expect("port missing");
-    println!("port open");
+    println!("connected over serial");
 
     let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0}));
+    let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));
 
     {
         let command = command.clone();
-        std::thread::spawn(move || server(command));
+        let telemetry = telemetry.clone();
+        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();
         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));
     }
 }
 
-fn server(command: Arc<Mutex<Command>>) {
+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 mut writer = BufWriter::new(writer);
         let mut reader = BufReader::new(stream);
 
         let mut data_buf = std::vec::Vec::new();
@@ -40,6 +57,14 @@ fn server(command: Arc<Mutex<Command>>) {
         *command.lock().unwrap() = new_command;
         println!("received");
 
+        let telem = telemetry.lock().unwrap().clone();
 
+        if telem.distance == f32::NAN {
+            continue;
+        }
+
+        serde_json::to_writer(&mut writer, &telem).unwrap();
+        writer.write(&[b'\n']).unwrap();
+        writer.flush().unwrap();
     }
 }

robot-controller/src/main.rs

@@ -3,6 +3,7 @@
 #![feature(abi_avr_interrupt)]
 
 use core::cell::RefCell;
+use core::f32;
 use core::mem::MaybeUninit;
 
 use arduino_hal::{clock::MHz16, hal::usart::UsartWriter};
@@ -12,7 +13,7 @@ use arduino_hal::pac::USART0;
 use arduino_hal::port::mode::*;
 use arduino_hal::prelude::*;
 use avr_device::interrupt::{self, Mutex};
-use common::Command;
+use common::{Command, Telemetry};
 use panic_halt as _;
 
 fn shift_out(data_pin: &mut Pin<Output, PB0>, clock_pin: &mut Pin<Output, PD4>, data: &u8) {
@@ -43,7 +44,6 @@ fn update_shift_register(
     latch_pin.set_high();
 }
 
-static mut UART_TX: MaybeUninit<UsartWriter<USART0, Pin<Input, PD0>, Pin<Output, PD1>, MHz16>> = MaybeUninit::uninit();
 static mut UART_RX: MaybeUninit<UsartReader<USART0, Pin<Input, PD0>, Pin<Output, PD1>, MHz16>> = MaybeUninit::uninit();
 
 static INPUT_LINE: Mutex<RefCell<[u8; 20]>> = Mutex::new(RefCell::new([0;20]));
@@ -51,7 +51,6 @@ static INPUT_LINE: Mutex<RefCell<[u8; 20]>> = Mutex::new(RefCell::new([0;20]));
 #[avr_device::interrupt(atmega328p)]
 unsafe fn USART_RX() {
     let rx = &mut *UART_RX.as_mut_ptr();
-    let tx = &mut *UART_TX.as_mut_ptr();
 
     static mut BUF: [u8; 20] = [0;20];
     static mut N: usize = 0; // index into line
@@ -106,6 +105,13 @@ fn main() -> ! {
     tc2.ocr2a.write(|w| w.bits(0));
     tc2.ocr2b.write(|w| w.bits(0));
 
+    let mut trig = pins.a4.into_output();
+    let echo = pins.a5;
+
+    let ultrasonic_timer = dp.TC1;
+    // 4us per tick
+    ultrasonic_timer.tccr1b.write(|w| w.cs1().prescale_64());
+
     let mut led = pins.d13.into_output();
 
     let mut serial = arduino_hal::default_serial!(dp, pins, 115200);
@@ -117,10 +123,11 @@ fn main() -> ! {
 
     unsafe { 
         UART_RX = MaybeUninit::new(rx);
-        UART_TX = MaybeUninit::new(tx);
         avr_device::interrupt::enable();
     };
 
+    let mut telem = Telemetry { distance: f32::NAN };
+
     loop {
         let mut shift_register = 0;
         const LEFT_FWD: u8 = 1 << 5; // 1a
@@ -156,7 +163,58 @@ fn main() -> ! {
 
         update_shift_register(&mut data_pin, &mut latch_pin, &mut clock_pin, &shift_register);
 
-        arduino_hal::delay_ms(1000);
+        // reset timer
+        ultrasonic_timer.tcnt1.write(|w| w.bits(0));
+
+        // 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
+            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);
     }
 }