From a3864c4ef0c6ab33f1633eb9e48f1436e3c3584f Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sat, 29 Mar 2025 19:02:58 -0400 Subject: [PATCH] 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 | 39 ++++++++++++++++++++++++--------- interface/src/main.rs | 45 ++++++++++++++++++++++++++------------ 2 files changed, 60 insertions(+), 24 deletions(-) diff --git a/interface/src/auto_impl.rs b/interface/src/auto_impl.rs index 45e9320..ea0eec9 100644 --- a/interface/src/auto_impl.rs +++ b/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, 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, tof_r: &mut 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 } } diff --git a/interface/src/main.rs b/interface/src/main.rs index 91f6e17..1e651b0 100644 --- a/interface/src/main.rs +++ b/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, 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, 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() { - control = auto_data.borrow_and_update().clone(); + 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)); } + + 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)); + 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) -> 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); }