From 0fa63e05120e77165976d0851be91f4b5554063f Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sat, 19 Jul 2025 15:59:50 -0500 Subject: [PATCH] ultrasonic enable toggle --- common/src/lib.rs | 1 + converter/Cargo.lock | 7 +++ converter/Cargo.toml | 1 + converter/src/main.rs | 78 +++++++++++++++++++----------- robot-controller/src/main.rs | 94 +++++++++++++++++++----------------- 5 files changed, 109 insertions(+), 72 deletions(-) diff --git a/common/src/lib.rs b/common/src/lib.rs index 88e818a..27b341d 100644 --- a/common/src/lib.rs +++ b/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)] diff --git a/converter/Cargo.lock b/converter/Cargo.lock index 3fd71a6..237f5a6 100644 --- a/converter/Cargo.lock +++ b/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", diff --git a/converter/Cargo.toml b/converter/Cargo.toml index df104d2..aa88434 100644 --- a/converter/Cargo.toml +++ b/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" diff --git a/converter/src/main.rs b/converter/src/main.rs index c4ae588..8a8e2d0 100644 --- a/converter/src/main.rs +++ b/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) - .timeout(Duration::from_secs(1)) + let mut port = serialport::new("/dev/ttyACM1", 115200) + .timeout(Duration::from_secs(1000)) .open().expect("port missing"); println!("connected over serial"); - let command: Arc> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0})); + let command: Arc> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: false})); let telemetry: Arc> = 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 = 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(telemetry: Arc>, mut reader: BufReader) { + 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::(&mut recv_buf[0..len]) { + println!("dist: {}", telem.distance); + *telemetry.lock().unwrap() = telem; + } + } +} + fn server(command: Arc>, telemetry: Arc>) { 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(); - 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"); - - 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(); + 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>, telemetry: Arc>, 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)?; + + let Ok(new_command) = serde_json::from_slice::(&data_buf[0..len]) else {continue;}; + + *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()?; } } diff --git a/robot-controller/src/main.rs b/robot-controller/src/main.rs index f73155b..c33c7bd 100644 --- a/robot-controller/src/main.rs +++ b/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 - 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(); + if command.ultrasonic_enable { + // reset timer 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); + // send ultrasonic pulse + trig.set_high(); + arduino_hal::delay_us(10); + trig.set_low(); - // inch per 148 microseconds - telem.distance = microseconds as f32 / 148.; - if microseconds == u16::MAX { telem.distance = f32::NAN }; + let mut response = true; - // 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 {} + /// 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); } 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.write(0); let _ = tx.flush(); - arduino_hal::delay_us(100); + arduino_hal::delay_us(500); } - - arduino_hal::delay_ms(250); } }