show ping on panel

the hardware is in really bad shape, the tofs randomly are stuck at max
and randomly stuck at min. The drive esc can't feed more than ~20%
power, and then not consistently either. Without any signal in the noise
it is impossible to move forward.

A possible full system redesign is to use a global shutter camera from
outside the box for all position data, perhaps supplemented with an
onboard IMU. tofs (at this price point) are not useful. It may be worth
trying new tofs that haven't had the chance for particulate to
accumulate in the emmiter/collector, but I'm not bullish on it.

interface/Cargo.lock

@@ -1571,12 +1571,48 @@ dependencies = [
  "percent-encoding",
 ]
 
+[[package]]
+name = "futures"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876"
+dependencies = [
+ "futures-channel",
+ "futures-core",
+ "futures-executor",
+ "futures-io",
+ "futures-sink",
+ "futures-task",
+ "futures-util",
+]
+
+[[package]]
+name = "futures-channel"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10"
+dependencies = [
+ "futures-core",
+ "futures-sink",
+]
+
 [[package]]
 name = "futures-core"
 version = "0.3.31"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e"
 
+[[package]]
+name = "futures-executor"
+version = "0.3.31"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f"
+dependencies = [
+ "futures-core",
+ "futures-task",
+ "futures-util",
+]
+
 [[package]]
 name = "futures-io"
 version = "0.3.31"
@@ -1625,6 +1661,7 @@ version = "0.3.31"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81"
 dependencies = [
+ "futures-channel",
  "futures-core",
  "futures-io",
  "futures-macro",
@@ -2179,6 +2216,7 @@ dependencies = [
  "home",
  "image",
  "libloading",
+ "ping-rs",
  "pitch-detection",
  "postcard",
  "rust-music-theory",
@@ -2470,6 +2508,18 @@ dependencies = [
  "simd-adler32",
 ]
 
+[[package]]
+name = "mio"
+version = "0.8.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a4a650543ca06a924e8b371db273b2756685faae30f8487da1b56505a8f78b0c"
+dependencies = [
+ "libc",
+ "log",
+ "wasi 0.11.0+wasi-snapshot-preview1",
+ "windows-sys 0.48.0",
+]
+
 [[package]]
 name = "mio"
 version = "1.0.3"
@@ -3049,6 +3099,19 @@ version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
 
+[[package]]
+name = "ping-rs"
+version = "0.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d873f038f84371f9c7fa13f6afea4d5f1fbcd5070ba8eb7af2a6d41c768eff8b"
+dependencies = [
+ "futures",
+ "mio 0.8.11",
+ "paste",
+ "socket2 0.4.10",
+ "windows 0.43.0",
+]
+
 [[package]]
 name = "piper"
 version = "0.2.4"
@@ -3671,6 +3734,16 @@ dependencies = [
  "serde",
 ]
 
+[[package]]
+name = "socket2"
+version = "0.4.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9f7916fc008ca5542385b89a3d3ce689953c143e9304a9bf8beec1de48994c0d"
+dependencies = [
+ "libc",
+ "winapi",
+]
+
 [[package]]
 name = "socket2"
 version = "0.5.8"
@@ -3926,11 +3999,11 @@ dependencies = [
  "backtrace",
  "bytes",
  "libc",
- "mio",
+ "mio 1.0.3",
  "parking_lot",
  "pin-project-lite",
  "signal-hook-registry",
- "socket2",
+ "socket2 0.5.8",
  "tokio-macros",
  "windows-sys 0.52.0",
 ]
@@ -4515,6 +4588,21 @@ version = "0.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"
 
+[[package]]
+name = "windows"
+version = "0.43.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "04662ed0e3e5630dfa9b26e4cb823b817f1a9addda855d973a9458c236556244"
+dependencies = [
+ "windows_aarch64_gnullvm 0.42.2",
+ "windows_aarch64_msvc 0.42.2",
+ "windows_i686_gnu 0.42.2",
+ "windows_i686_msvc 0.42.2",
+ "windows_x86_64_gnu 0.42.2",
+ "windows_x86_64_gnullvm 0.42.2",
+ "windows_x86_64_msvc 0.42.2",
+]
+
 [[package]]
 name = "windows"
 version = "0.54.0"
@@ -4632,6 +4720,15 @@ dependencies = [
  "windows-targets 0.42.2",
 ]
 
+[[package]]
+name = "windows-sys"
+version = "0.48.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9"
+dependencies = [
+ "windows-targets 0.48.5",
+]
+
 [[package]]
 name = "windows-sys"
 version = "0.52.0"

interface/Cargo.toml

@@ -25,3 +25,4 @@ libloading = "0.8.6"
 emath = "0.31.1"
 dashmap = { version = "6.1.0", features = ["serde"] }
 directories = "6.0.0"
+ping-rs = "0.1.2"

interface/src/auto_impl.rs

@@ -2,6 +2,7 @@
 use std::{collections::VecDeque, future::Future, ops::Sub, pin::Pin};
 
 use common::CamState;
+use tokio::time::Instant;
 use crate::auto::{AutoInterface, Configurable};
 
 
@@ -34,7 +35,6 @@ pub async fn auto(mut interface: AutoInterface) {
     let mut tof_r = Stats::new();
     loop {
         let data = interface.sensor_update().await;
-        println!("auto got on data");
         let cam = interface.cam_state();
         let CamState::Charged = cam else {
             continue;
@@ -51,12 +51,16 @@ pub async fn auto(mut interface: AutoInterface) {
 
         let auto_gap = interface.conf(&AUTO_GAP);
         let auto_self_occlusion = interface.conf(&AUTO_SELF_OCCLUSION);
+        let auto_range = interface.conf(&AUTO_SEEK_RANGE);
 
         let detection = |latest: u16, delta: i16| {
-            delta < auto_gap as i16 && latest > auto_self_occlusion as u16
+            -delta > auto_gap as i16 && 
+            latest > auto_self_occlusion as u16 &&
+            latest < auto_range as u16
         };
 
-        if detection(latest_tof_l, tof_l.delta()) || detection(latest_tof_l, tof_l.delta()) {
+
+        if dbg!(detection(latest_tof_l, tof_l.delta())) || dbg!(detection(latest_tof_r, dbg!(tof_r.delta()))) {
             if let Ok(()) = interface.enable() {
                 println!("found, now seek");
                 seek(interface.clone(), &mut tof_l, &mut tof_r).await;
@@ -70,11 +74,16 @@ pub async fn auto(mut interface: AutoInterface) {
 async fn seek(mut interface: AutoInterface, tof_l: &mut Stats<i16>, tof_r: &mut Stats<i16>) {
     let crossover = interface.conf(&AUTO_CONVERGENCE_POINT) as i16; 
     let range = interface.conf(&AUTO_SEEK_RANGE) as i16;
+    let mut timeout = Instant::now();
     loop {
         let data = interface.sensor_data();
         data.tof_l.map(|d| tof_l.update(d as i16));
         data.tof_r.map(|d| tof_r.update(d as i16));
 
+        if data.tof_l.is_none() || data.tof_r.is_none() {
+            return;
+        }
+
         let left_near = tof_l.latest() < crossover && tof_l.delta() < 70;
         let right_near = tof_r.latest() < crossover && tof_r.delta() < 70;
 
@@ -85,19 +94,45 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats<i16>, tof_r: &mut
 
         let far = !near && (left_far || right_far);
 
+        if !(near || far) {
+            if timeout.elapsed().as_millis() > 1200 {
+                return;
+            }
+        } else {
+            timeout = Instant::now();
+        }
+
         let mut twist = 0.0;
         if near {
-            if tof_l.max() > tof_r.max() {
-                twist = 1.0; // right
+            // if one is beyond the convergence point use far logic
+            if tof_l.max().max(tof_r.max()) > 
+                interface.conf(&AUTO_CONVERGENCE_POINT) as i16 {
+                if right_near {
+                    twist = -0.7;
+                } else if left_near {
+                    twist = 0.7;
+                }
             } else {
-                twist = -1.0; // left
+                // go towards the further side
+                let mut diff = tof_l.max() - tof_r.max();
+                diff = diff.max(-100).min(100);
+
+                twist = (diff as f32) / 150.;
+
             }
         } else if far {
-            if tof_l.max() > tof_r.max() {
-                twist = -1.0; // left
-            } else {
-                twist = 1.0; // right
+            if tof_r.max() - tof_l.max() > 100 {
+                twist = 0.7; // right high, go right
             }
+            if tof_l.max() - tof_r.max() > 100 {
+                twist = -0.7; // left high, go left
+            }
+        }
+
+        if should_fire(&tof_l, &tof_r) {
+            let _ = interface.run_command(common::ControlPacket::Fire);
+            println!("fired");
+            return;
         }
 
         let _ = interface.run_command(common::ControlPacket::Twist(1.0, twist));
@@ -105,6 +140,11 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats<i16>, tof_r: &mut
     }
 }
 
+fn should_fire(left: &Stats<i16>, right: &Stats<i16>) -> bool {
+    left.latest() < 70 && left.latest() > 20 && left.delta() < 60 &&
+    right.latest() < 70 && right.latest() > 20 && right.delta() < 60
+}
+
 struct Stats<T> {
     table: VecDeque<T>
 }

interface/src/gui.rs

@@ -6,11 +6,11 @@ use image::ImageFormat;
 use tokio::{runtime::Runtime, sync::{mpsc, watch::{self, Receiver}}};
 use egui_toast::{Toast, Toasts};
 
-use crate::{auto::Configurable, auto_impl::CONFIGS, storage::StorageManager, POWER_THRESHOLD};
+use crate::{auto::Configurable, auto_impl::CONFIGS, storage::StorageManager, PingData, POWER_THRESHOLD, RECONNECT};
 
 pub const GUI: OnceCell<Context> = OnceCell::new();
 
-pub fn gui(data: Receiver<GUIData>, toasts: mpsc::Receiver<Toast>, executor: Runtime, autoconf: watch::Receiver<&'static [Configurable]>, storage: StorageManager, auto_allowed: Arc<AtomicBool>, auto_enabled: Arc<AtomicBool>) -> eframe::Result {
+pub fn gui(data: Receiver<GUIData>, toasts: mpsc::Receiver<Toast>, executor: Runtime, autoconf: watch::Receiver<&'static [Configurable]>, storage: StorageManager, auto_allowed: Arc<AtomicBool>, auto_enabled: Arc<AtomicBool>, ping: PingData) -> eframe::Result {
     let icon = egui::include_image!("../assets/lizard.png");
 
     let icon = image::load_from_memory_with_format(include_bytes!("../assets/lizard.png"), ImageFormat::Png).unwrap();
@@ -35,7 +35,7 @@ pub fn gui(data: Receiver<GUIData>, toasts: mpsc::Receiver<Toast>, executor: Run
             // This gives us image support:
             egui_extras::install_image_loaders(&cc.egui_ctx);
 
-            Ok(Box::new(GUI::with_receivers(data, toasts, executor, storage, autoconf, auto_allowed, auto_enabled)))
+            Ok(Box::new(GUI::with_receivers(data, toasts, executor, storage, autoconf, auto_allowed, auto_enabled, ping)))
         }),
     )
 }
@@ -65,10 +65,11 @@ struct GUI {
     storage: StorageManager,
     auto_allowed: Arc<AtomicBool>,
     auto_enabled: Arc<AtomicBool>,
+    ping: PingData,
 }
 
 impl GUI {
-    fn with_receivers(data: Receiver<GUIData>, toasts: mpsc::Receiver<Toast>, executor: Runtime, storage: StorageManager, autoconf: watch::Receiver<&'static [Configurable]>, auto_allowed: Arc<AtomicBool>, auto_enabled: Arc<AtomicBool>) -> Self {
+    fn with_receivers(data: Receiver<GUIData>, toasts: mpsc::Receiver<Toast>, executor: Runtime, storage: StorageManager, autoconf: watch::Receiver<&'static [Configurable]>, auto_allowed: Arc<AtomicBool>, auto_enabled: Arc<AtomicBool>, ping: PingData) -> Self {
         Self {
             data,
             toasts,
@@ -78,6 +79,7 @@ impl GUI {
             storage,
             auto_allowed,
             auto_enabled,
+            ping,
         }
     }
 }
@@ -105,11 +107,27 @@ impl eframe::App for GUI {
         egui::CentralPanel::default().show(ctx, |ui| {
             ui.heading("Cruise Control");
 
+            if !RECONNECT.load(Ordering::Acquire) {
+                if ui.button("disconnect").clicked() {
+                    RECONNECT.store(true, Ordering::Release);
+                }
+            }
+
+            if let Ok(ping) = self.ping.try_read() {
+                ui.horizontal(|ui| {
+                    ui.label(format!("router: {}", if let Some(rtt) = ping.0 {format!("✅ {rtt}ms")} else {"❌".into()}));
+                    ui.label(format!("robot: {}", if let Some(rtt) = ping.1 {format!("✅ {rtt}ms")} else {"❌".into()}));
+                });
+            }
+
+
             if let Some(ref command) = self.data.borrow().last_command {
                 ui.label(format!("sending {command:?}"));
             }
+            ui.horizontal(|ui| {
                 ui.label(format!("auto authorized: {}", if self.auto_allowed.load(Ordering::Acquire) {"✅"} else {"❌"}));
                 ui.label(format!("auto running: {}", if self.auto_enabled.load(Ordering::Acquire) {"✅ zoom vroom"} else {"❌"}));
+            });
 
             if let Some(ref telem) = self.data.borrow().telemetry {
                     ui.label(format!("Left tof: {}", if let Some(tof) = telem.sensors.tof_l {format!("✅ {tof}mm")} else {"❌".into()}));

interface/src/lib.rs

@@ -1,7 +1,10 @@
 #![feature(iter_collect_into)]
 
+use std::sync::{atomic::AtomicBool, Arc};
+
 use atomic_float::AtomicF32;
 use gui::DEFAULT_VOLUME_THRESHOLD;
+use tokio::sync::RwLock;
 
 pub mod gui;
 pub mod combatlog;
@@ -11,3 +14,6 @@ pub mod storage;
 
 pub const POWER_THRESHOLD: AtomicF32 = AtomicF32::new(DEFAULT_VOLUME_THRESHOLD);
 
+pub static RECONNECT: AtomicBool = AtomicBool::new(false);
+
+pub type PingData = Arc<RwLock<(Option<u32>, Option<u32>)>>;

interface/src/main.rs

@@ -1,16 +1,16 @@
-use std::{fmt::format, ops::ControlFlow, path::Path, result, sync::{atomic::{AtomicBool, Ordering}, Arc}, thread::{self, sleep}, time::Duration};
+use std::{fmt::format, net::{IpAddr, Ipv4Addr}, ops::ControlFlow, path::Path, result, sync::{atomic::{AtomicBool, Ordering}, Arc}, thread::{self, sleep}, time::Duration};
 
-use anyhow::{Context, Ok, Result};
+use anyhow::{anyhow, Context, Ok, Result};
 use atomic_float::AtomicF32;
 use directories::ProjectDirs;
-use interface::{auto::{get_confs, Auto, AutoInterface}, auto_impl, combatlog::{combat_logger, CombatData}, gui::CONFIGS, storage::StorageManager, POWER_THRESHOLD};
+use interface::{auto::{get_confs, Auto, AutoInterface}, auto_impl::{self, CONFIGS}, combatlog::{combat_logger, CombatData}, storage::StorageManager, PingData, POWER_THRESHOLD, RECONNECT};
 use common::{ControlPacket, TelemetryPacket};
 use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
 use egui_toast::{Toast, ToastKind};
 use interface::gui::{gui, GUIData, DEFAULT_VOLUME_THRESHOLD, GUI};
 use pitch_detection::{detector::{mcleod::McLeodDetector, PitchDetector}, utils};
 use rust_music_theory::note::{Note, NoteLetter, Pitch, Tuning};
-use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, spawn, sync::{self, broadcast, mpsc, watch, RwLock}, time::timeout};
+use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, join, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, spawn, sync::{self, broadcast, mpsc, watch, RwLock}, time::{sleep_until, timeout, Instant}};
 
 
 fn main() -> Result<()> {
@@ -22,7 +22,9 @@ fn main() -> Result<()> {
     let auto_allowed = Arc::new(AtomicBool::new(false));
 
     let (auto_telem_sender, auto_telem) = watch::channel(TelemetryPacket::default());
-    let (interface, auto_command, auto_enabled) = AutoInterface::new(auto_telem.clone(), storage.clone(), auto_allowed.clone());
+    let (interface, _auto_command, auto_enabled) = AutoInterface::new(auto_telem.clone(), storage.clone(), auto_allowed.clone());
+
+    let ping: PingData = Arc::new(RwLock::new((None, None)));
 
     println!("name: {}", auto.name());
 
@@ -101,8 +103,42 @@ fn main() -> Result<()> {
     let spawner = executor.handle().clone();
     let auto_allowed_ = auto_allowed.clone();
     let interface_ = interface.clone();
+    let ping_ = ping.clone();
     thread::spawn(move || {
         spawner.block_on(async {
+            tokio::spawn(async move {
+                let robot: IpAddr = "192.168.1.2".parse().unwrap();
+                let router: IpAddr = "192.168.1.1".parse().unwrap();
+
+                loop {
+                    let start = Instant::now();
+                    let (robot, router) = join!(
+                        ping_rs::send_ping_async(&robot, 
+                            Duration::from_millis(250), 
+                            Arc::new(&[0,1,2,3]), 
+                            None,
+                        ),
+                        ping_rs::send_ping_async(&router, 
+                            Duration::from_millis(250), 
+                            Arc::new(&[0,1,2,3]), 
+                            None,
+                        ),
+                    );
+
+                    let robot = robot.ok().map(|r| r.rtt);
+                    let router = router.ok().map(|r| r.rtt);
+
+                    let mut ping = ping_.write().await;
+
+                    ping.0 = router;
+                    ping.1 = robot;
+
+                    drop(ping);
+
+                    sleep_until(start + Duration::from_millis(500)).await;
+                    
+                }
+            });
 
             let log_toasts = toast_sender.clone();
             tokio::spawn(async move {
@@ -117,6 +153,7 @@ fn main() -> Result<()> {
                         break;
                     };
                 }
+                let _ = data_sender.send(GUIData { telemetry: None, last_command: None });
             }
         });
 
@@ -133,7 +170,7 @@ fn main() -> Result<()> {
 
     println!("launching gui");
     let (_conf_sender, conf) = watch::channel(CONFIGS);
-    gui(gui_data, toasts, executor, conf, storage.clone(), auto_allowed, auto_enabled).unwrap();
+    gui(gui_data, toasts, executor, conf, storage.clone(), auto_allowed, auto_enabled, ping.clone()).unwrap();
 
     drop(stream);
 
@@ -192,9 +229,13 @@ async fn controller(mut notes: broadcast::Receiver<Detection>, controller: Owned
     let mut auto_data = interface.subscribe();
 
     loop {
-        let mut control = ControlPacket::Stop;
+        let mut control = ControlPacket::Twist(0.0, 0.0);
 
-        let result::Result::Ok(note) = timeout(Duration::from_millis(250), notes.recv()).await else {
+        let auto_enabled = || {enable.load(Ordering::Acquire) && interface.enabled()};
+
+        let control_timeout = if auto_enabled() {Duration::from_millis(50)} else {Duration::from_millis(350)};
+
+        let result::Result::Ok(note) = timeout(control_timeout, notes.recv()).await else {
             println!("timeout");
             send_packet(&mut controller, ControlPacket::Stop).await?;
             continue;
@@ -203,24 +244,42 @@ async fn controller(mut notes: broadcast::Receiver<Detection>, controller: Owned
         let (note,vol) = note.context("channel closed")?;
         if let Some(note) = note {
             dbg!(note);
+            control = ControlPacket::Stop;
             if let ControlFlow::Break(_) = sax_control(&mut control, vol, note, enable.clone()) {
                 if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note, enable.clone()) {
                     continue;
                 }
             }
 
-            if enable.load(Ordering::Acquire) && interface.enabled() {
+            if let ControlPacket::Twist(ref mut f,ref mut r) = control {
+                let scale = 0.8;
+                *f *= scale;
+                *r *= scale;
+            }
+        }
+
+        if auto_enabled() {
             control = auto_data.borrow_and_update().clone();
+            
+            if let ControlPacket::Twist(ref mut f,ref mut r) = control {
+                let scale = 0.1;
+                *f *= scale;
+                *r *= scale;
+            }
+
         }
 
         send_packet(&mut controller, control.clone()).await?;
         let _ = logging_sender.send(CombatData::Control(control.clone())).await;
         gui.send_modify(|gui| gui.last_command = Some(control));
-        } else {
-            send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
-            gui.send_modify(|gui| gui.last_command = Some(ControlPacket::Stop));
-        }
+
         GUI.get().map(|c| c.request_repaint());
+
+        if RECONNECT.load(Ordering::Acquire) {
+            RECONNECT.store(false, Ordering::Release);
+            return Ok(());
+        }
+
     }
 }
 
@@ -281,7 +340,7 @@ fn sax_control(control: &mut ControlPacket, vol: f32, frequency: f32, enable: Ar
 
 
 fn recorder_control(control: &mut ControlPacket, vol: f32, frequency: f32, enable: Arc<AtomicBool>) -> ControlFlow<()> {
-    if frequency < 300. {
+    if frequency < 280. {
         println!("too low");
         return ControlFlow::Break(());
     }
@@ -318,8 +377,9 @@ fn recorder_control(control: &mut ControlPacket, vol: f32, frequency: f32, enabl
         Pitch { letter: NoteLetter::F, accidental: 1}  => {
             println!("stop flat");
             *control = ControlPacket::Stop;
+            enable.store(false, Ordering::Release);
         }
-        Pitch { letter: NoteLetter::F, accidental: 0}  => {
+        Pitch { letter: NoteLetter::D, accidental: _}  => {
             println!("auto en");
             enable.store(true, Ordering::Release);
         }