fire in auto

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<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));
@@ -85,6 +87,14 @@ 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() {
@@ -100,11 +110,22 @@ async fn seek(mut interface: AutoInterface, tof_l: &mut Stats<i16>, 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<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>
 }