ank/cruisecontrol
1
Fork 0
Code Issues Activity

fire in auto

Browse source
This commit is contained in:
Andy Killorin 2025-03-28 18:35:40 -04:00
parent 17f484acd5
commit 7fe4e4477e
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 21 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

21
interface/src/auto_impl.rs
View file

@ -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>
}