diff --git a/interface/src/auto.rs b/interface/src/auto.rs index 3c075d7..746a407 100644 --- a/interface/src/auto.rs +++ b/interface/src/auto.rs @@ -1,4 +1,4 @@ -use std::{future::Future, ops::{Deref, Range}, pin::Pin, sync::{atomic::{AtomicBool, Ordering}, Arc}}; +use std::{future::Future, ops::{Deref, Range}, pin::Pin, sync::{atomic::{AtomicBool, Ordering}, Arc}, thread::sleep, time::Duration}; use common::{CamState, ControlPacket, SensorData, TelemetryPacket}; use anyhow::{anyhow, Ok, Result}; @@ -53,7 +53,15 @@ impl AutoInterface { pub async fn sensor_update(&mut self) -> SensorData { self.data_receiver.changed().await.unwrap(); self.data_receiver.borrow().sensors.clone() - + } + pub fn sensor_update_blocking(&mut self) -> SensorData { + loop { + if self.data_receiver.has_changed().unwrap() { + break; + } + sleep(Duration::from_millis(10)); // TODO: exponential or condvars + } + self.data_receiver.borrow_and_update().sensors.clone() } /// disable auto pub fn disable(&mut self) { @@ -140,12 +148,12 @@ impl Auto { } /// entrypoint - pub async fn run(&self, interface: AutoInterface) -> Result> { - let func: Symbol Pin + Send>>> = unsafe {self.library.get(b"entry")?}; + pub fn run(&self, interface: AutoInterface) -> Result<()> { + let func: Symbol ()> = unsafe {self.library.get(b"entry")?}; - let fut = unsafe { func(interface.clone()) }; + unsafe { func(interface.clone()) }; - Ok(tokio::spawn(fut)) + Ok(()) } pub fn configs(&self) -> &'static [Configurable] { diff --git a/interface/src/auto_impl.rs b/interface/src/auto_impl.rs new file mode 100644 index 0000000..5c5e161 --- /dev/null +++ b/interface/src/auto_impl.rs @@ -0,0 +1,146 @@ + +use std::{collections::VecDeque, future::Future, ops::Sub, pin::Pin}; + +use common::CamState; +use crate::auto::{AutoInterface, Configurable}; + + +#[unsafe(no_mangle)] +pub fn entry(interface: AutoInterface) -> Pin + Send>> { + Box::pin(auto(interface)) +} + +#[unsafe(no_mangle)] +pub static NAME: &'static str = "scanseek v1"; + +const AUTO_GAP: Configurable = Configurable::new("auto minimum gap").range(0. .. 300.).default(140.) + .description("distance (mm) distance measurements must instantaneously drop to indicate a detection. This should line up with the size of the smallest robot you compete against"); +const AUTO_SELF_OCCLUSION: Configurable = Configurable::new("auto self occlusion").range(0. .. 200.).default(143.) + .description("distance (mm) below which measurements are considered noise in the scan phase"); +const AUTO_CONVERGENCE_POINT: Configurable = Configurable::new("auto convergence").range(100. .. 300.).default(190.) + .description("distance (mm) where the tof beams intersect"); +const AUTO_SEEK_RANGE: Configurable = Configurable::new("auto seek range").range(200. .. 8000.).default(500.); + +#[unsafe(no_mangle)] +pub static CONFIGS: &[Configurable] = &[ + AUTO_GAP, + AUTO_SELF_OCCLUSION, + AUTO_CONVERGENCE_POINT, + AUTO_SEEK_RANGE, +]; + +pub async fn auto(mut interface: AutoInterface) { + let mut tof_l = Stats::new(); + 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; + }; + + let Some(latest_tof_l) = data.tof_l else { + continue; + }; + tof_l.update(latest_tof_l as i16); + let Some(latest_tof_r) = data.tof_r else { + continue; + }; + tof_r.update(latest_tof_r as i16); + + let auto_gap = interface.conf(&AUTO_GAP); + let auto_self_occlusion = interface.conf(&AUTO_SELF_OCCLUSION); + + let detection = |latest: u16, delta: i16| { + delta < auto_gap as i16 && latest > auto_self_occlusion as u16 + }; + + if detection(latest_tof_l, tof_l.delta()) || detection(latest_tof_l, tof_l.delta()) { + if let Ok(()) = interface.enable() { + println!("found, now seek"); + seek(interface.clone(), &mut tof_l, &mut tof_r).await; + } else { + println!("requested enable") + } + } + } +} + +async fn seek(mut interface: AutoInterface, tof_l: &mut Stats, tof_r: &mut Stats) { + let crossover = interface.conf(&AUTO_CONVERGENCE_POINT) as i16; + let range = interface.conf(&AUTO_SEEK_RANGE) as i16; + 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)); + + let left_near = tof_l.latest() < crossover && tof_l.delta() < 70; + let right_near = tof_r.latest() < crossover && tof_r.delta() < 70; + + let left_far = tof_l.latest() > crossover && tof_l.latest() < range && tof_l.delta() < 70; + let right_far = tof_r.latest() > crossover && tof_r.latest() < range && tof_r.delta() < 70; + + let near = left_near || right_near; + + let far = !near && (left_far || right_far); + + let mut twist = 0.0; + if near { + if tof_l.max() > tof_r.max() { + twist = 1.0; // right + } else { + twist = -1.0; // left + } + } else if far { + if tof_l.max() > tof_r.max() { + twist = -1.0; // left + } else { + twist = 1.0; // right + } + } + + let _ = interface.run_command(common::ControlPacket::Twist(1.0, twist)); + interface.sensor_update_blocking(); + } +} + +struct Stats { + table: VecDeque +} + +impl Stats { + pub fn new() -> Self { + Self { table: VecDeque::new() } + + } + + const MAX_ELEMENTS: usize = 3; + + pub fn update(&mut self, elem: T) { + self.table.push_front(elem); + if self.table.len() > Self::MAX_ELEMENTS { + self.table.pop_back(); + } + } + + /// panics if no values have been written + pub fn latest(&self) -> T { + self.table.front().unwrap().to_owned() + } +} + +impl + Copy + From> Stats { + pub fn delta(&self) -> T { + *self.table.get(0).unwrap_or(&T::from(0)) - *self.table.get(1).unwrap_or(&T::from(0)) + } + + pub fn max(&self) -> T { + *self.table.iter().max().unwrap_or(&T::from(0)) + } + + pub fn min(&self) -> T { + *self.table.iter().max().unwrap_or(&T::from(0)) + } + +} diff --git a/interface/src/gui.rs b/interface/src/gui.rs index 28a7e26..09884bd 100644 --- a/interface/src/gui.rs +++ b/interface/src/gui.rs @@ -6,7 +6,7 @@ use image::ImageFormat; use tokio::{runtime::Runtime, sync::{mpsc, watch::{self, Receiver}}}; use egui_toast::{Toast, Toasts}; -use crate::{auto::Configurable, storage::StorageManager, POWER_THRESHOLD}; +use crate::{auto::Configurable, auto_impl::CONFIGS, storage::StorageManager, POWER_THRESHOLD}; pub const GUI: OnceCell = OnceCell::new(); @@ -82,17 +82,6 @@ impl GUI { } } -// dupe from auto crate for testing -const AUTO_GAP: Configurable = Configurable::new("auto minimum gap").range(0. .. 300.).default(140.) - .description("distance (mm) distance measurements must instantaneously drop to indicate a detection. This should line up with the size of the smallest robot you compete against"); -const AUTO_SELF_OCCLUSION: Configurable = Configurable::new("auto self occlusion").range(0. .. 200.).default(143.) - .description("distance (mm) below which measurements are considered noise in the scan phase"); - -pub static CONFIGS: &[Configurable] = &[ - AUTO_GAP, - AUTO_SELF_OCCLUSION, -]; - impl eframe::App for GUI { fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) { let _ = GUI.set(ctx.clone()); diff --git a/interface/src/lib.rs b/interface/src/lib.rs index 8f92088..237d105 100644 --- a/interface/src/lib.rs +++ b/interface/src/lib.rs @@ -6,6 +6,7 @@ use gui::DEFAULT_VOLUME_THRESHOLD; pub mod gui; pub mod combatlog; pub mod auto; +pub mod auto_impl; pub mod storage; pub const POWER_THRESHOLD: AtomicF32 = AtomicF32::new(DEFAULT_VOLUME_THRESHOLD); diff --git a/interface/src/main.rs b/interface/src/main.rs index 8fdfe58..91f6e17 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}, combatlog::{combat_logger, CombatData}, gui::CONFIGS, storage::StorageManager, POWER_THRESHOLD}; +use interface::{auto::{get_confs, Auto, AutoInterface}, auto_impl, combatlog::{combat_logger, CombatData}, gui::CONFIGS, storage::StorageManager, POWER_THRESHOLD}; use common::{ControlPacket, TelemetryPacket}; use cpal::traits::{DeviceTrait, HostTrait, StreamTrait}; use egui_toast::{Toast, ToastKind}; @@ -126,7 +126,7 @@ fn main() -> Result<()> { let spawner = executor.handle().clone(); thread::spawn(move || { spawner.block_on(async { - let _stoppie = auto.run(interface_).await.unwrap(); + let _stoppie = auto_impl::auto(interface_).await; println!("auto ended"); }); });