procedural coinflip

Author: SarangaR

.envrc

@@ -0,0 +1 @@
+use flake ./#noFHS

src/config/coinflip.rs

@@ -0,0 +1,18 @@
+use serde::{Deserialize, Serialize};
+
+#[derive(Debug, Serialize, Deserialize)]
+pub struct Config {
+    pub depth: f32,
+    pub angle_correction: f32,
+    pub true_count: u32,
+}
+
+impl Default for Config {
+    fn default() -> Self {
+        Self {
+            depth: -1.25,
+            angle_correction: 15.0,
+            true_count: 4,
+        }
+    }
+}

src/config/mod.rs

@@ -1,4 +1,5 @@
 pub mod bin;
+pub mod coinflip;
 pub mod gate;
 pub mod octagon;
 pub mod path_align;
@@ -76,6 +77,7 @@ pub struct Missions {
     pub slalom: slalom::Config,
     pub bin: bin::Config,
     pub octagon: octagon::Config,
+    pub coinflip: coinflip::Config,
 }
 
 #[derive(Debug, Serialize, Deserialize)]

src/main.rs

@@ -20,7 +20,7 @@ use sw8s_rust_lib::{
         circle_buoy::{
             buoy_circle_sequence, buoy_circle_sequence_blind, buoy_circle_sequence_model,
         },
-        coinflip::coinflip,
+        coinflip::{coinflip, coinflip_procedural},
         example::{initial_descent, pid_test},
         fire_torpedo::{FireLeftTorpedo, FireRightTorpedo},
         gate::{gate_run_complex, gate_run_naive, gate_run_procedural, gate_run_testing},
@@ -467,7 +467,12 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> {
             FireLeftTorpedo::new(static_context().await).execute().await;
             Ok(())
         }
-        "coinflip" => ctwrap!(coinflip(static_context().await).execute()),
+        "coinflip" => {
+            ctwrap!(coinflip_procedural(
+                static_context().await,
+                &config.missions.coinflip
+            ))
+        }
         // Just stall out forever
         "forever" | "infinite" => loop {
             while control_board().await.raw_speed_set([0.0; 8]).await.is_err() {}

src/missions/coinflip.rs

@@ -4,6 +4,7 @@ use tokio_serial::SerialStream;
 
 use crate::{
     act_nest,
+    config::coinflip::Config,
     missions::{
         extra::AlwaysTrue,
         meb::WaitArm,
@@ -33,6 +34,7 @@ pub async fn coinflip_procedural<
     Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
 >(
     context: &Con,
+    config: &Config,
 ) {
     #[cfg(feature = "logging")]
     logln!("Starting path align");
@@ -51,10 +53,17 @@ pub async fn coinflip_procedural<
         }
     };
 
+    let DEPTH = config.depth;
+
     let _ = cb
-        .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, 0.0)
+        .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, DEPTH)
         .await;
 
+    let mut true_count = 0;
+    let max_true_count = config.true_count;
+    let mut target_yaw = initial_yaw;
+    let angle_correction = config.angle_correction;
+
     loop {
         #[allow(unused_variables)]
         let detections = vision.execute().await.unwrap_or_else(|e| {
@@ -63,10 +72,33 @@ pub async fn coinflip_procedural<
             vec![]
         });
 
-        let _gate = detections
+        let gate = detections
             .iter()
             .filter(|d| matches!(d.class().identifier, Target::Gate))
             .collect_vec();
+        let shark = detections
+            .iter()
+            .filter(|d| matches!(d.class().identifier, Target::Shark))
+            .collect_vec();
+        let sawfish = detections
+            .iter()
+            .filter(|d| matches!(d.class().identifier, Target::Sawfish))
+            .collect_vec();
+
+        if (gate.len() > 0 || shark.len() > 0 || sawfish.len() > 0) {
+            if true_count > max_true_count {
+                let _ = cb
+                    .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, DEPTH)
+                    .await;
+            } else {
+                true_count += 1;
+            }
+        } else {
+            target_yaw += angle_correction;
+            let _ = cb
+                .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, target_yaw, DEPTH)
+                .await;
+        }
     }
 }
 
@@ -113,10 +145,11 @@ pub fn coinflip<
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::Gate),
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::Middle),
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::LeftPole),
-                    DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::RightPole),
+                    DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(
+                        Target::RightPole
+                    ),
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::Shark),
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::Sawfish),
-
                 ),
                 CountTrue::new(TRUE_COUNT),
             ),