diff --git a/interface/src/auto_impl.rs b/interface/src/auto_impl.rs index 5c5e161..45e9320 100644 --- a/interface/src/auto_impl.rs +++ b/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}; @@ -70,6 +71,7 @@ pub async fn auto(mut interface: AutoInterface) { 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; + let mut timeout = Instant::now(); loop { let data = interface.sensor_data(); data.tof_l.map(|d| tof_l.update(d as i16)); @@ -85,6 +87,14 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats, 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() { @@ -100,11 +110,22 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats, tof_r: &mut } } + 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)); interface.sensor_update_blocking(); } } +fn should_fire(left: &Stats, right: &Stats) -> bool { + left.latest() < 70 && left.latest() > 20 && left.delta() < 60 && + right.latest() < 70 && right.latest() > 20 && right.delta() < 60 +} + struct Stats { table: VecDeque }