use past convergence logic when one sensor is pat the convergence point

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/src/auto_impl.rs

@@ -35,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;
@@ -52,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;
@@ -77,6 +80,10 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats<i16>, tof_r: &mut
         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;
 
@@ -97,16 +104,28 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats<i16>, tof_r: &mut
 
         let mut twist = 0.0;
         if near {
-            if tof_l.max() > tof_r.max() {
+            // if one is beyond the convergence point use far logic
-                twist = 1.0; // right
+            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() {
+            if tof_r.max() - tof_l.max() > 100 {
-                twist = -1.0; // left
+                twist = 0.7; // right high, go right
-            } else {
+            }
-                twist = 1.0; // right
+            if tof_l.max() - tof_r.max() > 100 {
+                twist = -0.7; // left high, go left
             }
         }
 

interface/src/main.rs

@@ -3,7 +3,7 @@ use std::{fmt::format, ops::ControlFlow, path::Path, result, sync::{atomic::{Ato
 use 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, POWER_THRESHOLD};
 use common::{ControlPacket, TelemetryPacket};
 use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
 use egui_toast::{Toast, ToastKind};
@@ -22,7 +22,7 @@ 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());
 
     println!("name: {}", auto.name());
 
@@ -192,9 +192,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,23 +207,35 @@ 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());
     }
 }
@@ -281,7 +297,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 +334,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);
         }