cruisecontrol/interface/src/auto_impl.rs

use std::{collections::VecDeque, future::Future, ops::Sub, pin::Pin};

use common::CamState;
use tokio::time::Instant;
use crate::auto::{AutoInterface, Configurable};


#[unsafe(no_mangle)]
pub fn entry(interface: AutoInterface) -> Pin<Box<dyn Future<Output = ()> + 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;
        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 auto_range = interface.conf(&AUTO_SEEK_RANGE);

        let detection = |latest: u16, delta: i16| {
            -delta > auto_gap as i16 && 
            latest > auto_self_occlusion as u16 &&
            latest < auto_range as u16
        };


        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;
            } else {
                println!("requested enable")
            }
        }
    }
}

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));
        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;

        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);

        if !(near || far) {
            if timeout.elapsed().as_millis() > 1200 {
                return;
            }
        } else {
            timeout = Instant::now();
        }

        let mut twist = 0.0;
        if near {
            // 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 {
                // 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_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
            }
        }

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

impl<T: Copy> Stats<T> {
    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<T: Ord + Sub<Output = T> + Copy + From<u8>> Stats<T> {
    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))
    }

}