post semi run 1

Author: Cowboylaserkittenjetshark

src/config/path_align.rs

@@ -5,7 +5,11 @@ pub struct Config {
     pub depth: f32,
     pub speed: f32,
     pub forward_speed: f32,
+    pub strafe_speed: f32,
     pub detections: u8,
+    pub yaw_angle: f32,
+    pub forward_duration: u64,
+    pub yaw_wait: u64,
 }
 
 impl Default for Config {
@@ -14,7 +18,11 @@ impl Default for Config {
             depth: -1.25,
             speed: 0.3,
             forward_speed: 0.3,
+            strafe_speed: 0.3,
             detections: 10,
+            yaw_angle: 15.0,
+            forward_duration: 3,
+            yaw_wait: 3,
         }
     }
 }

src/main.rs

@@ -26,7 +26,7 @@ use sw8s_rust_lib::{
         gate::{gate_run_complex, gate_run_naive, gate_run_procedural, gate_run_testing},
         meb::WaitArm,
         octagon::octagon,
-        path_align::path_align_procedural,
+        path_align::{path_align_procedural, static_align_procedural},
         reset_torpedo::ResetTorpedo,
         slalom::slalom,
         sonar::sonar,
@@ -422,6 +422,16 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> {
             &config.missions.path_align,
             &config.get_color_profile().unwrap(),
         )),
+        "static_align" => ctwrap!(static_align_procedural(
+            &FullActionContext::new(
+                control_board().await,
+                meb().await,
+                front_cam().await,
+                bottom_cam().await,
+                gate_target().await,
+            ),
+            &config.missions.path_align,
+        )),
         "example" => ctwrap!(initial_descent(&FullActionContext::new(
             control_board().await,
             meb().await,

src/missions/gate.rs

@@ -89,12 +89,12 @@ pub async fn gate_run_procedural<
 
         let shark = detections
             .iter()
-            .filter(|d| matches!(d.class().identifier, Target::Shark))
+            .filter(|d| matches!(d.class().identifier, Target::Sawfish))
             .collect_vec();
 
         let sawfish = detections
             .iter()
-            .filter(|d| matches!(d.class().identifier, Target::Sawfish))
+            .filter(|d| matches!(d.class().identifier, Target::Shark))
             .collect_vec();
 
         let mut traversal_timer = DelayAction::new(8.0); // forward duration in second

src/missions/path_align.rs

@@ -125,3 +125,50 @@ pub async fn path_align_procedural<
         .await;
     sleep(Duration::from_secs(1)).await;
 }
+
+pub async fn static_align_procedural<
+    Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + BottomCamIO,
+>(
+    context: &Con,
+    config: &Config,
+) {
+    #[cfg(feature = "logging")]
+    logln!("Starting static align");
+
+    let cb = context.get_control_board();
+    let _ = cb.bno055_periodic_read(true).await;
+
+    let initial_yaw = loop {
+        if let Some(initial_angle) = cb.responses().get_angles().await {
+            break *initial_angle.yaw();
+        } else {
+            #[cfg(feature = "logging")]
+            logln!("Failed to get initial angle");
+        }
+    };
+
+    let target_yaw = initial_yaw + config.yaw_angle;
+
+    let _ = cb
+        .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth)
+        .await;
+
+    sleep(Duration::from_secs(config.yaw_wait)).await;
+
+    let _ = cb
+        .stability_2_speed_set(
+            config.strafe_speed,
+            config.forward_speed,
+            0.0,
+            0.0,
+            target_yaw,
+            config.depth,
+        )
+        .await;
+
+    sleep(Duration::from_secs(config.forward_duration)).await;
+
+    let _ = cb
+        .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, target_yaw, config.depth)
+        .await;
+}

src/missions/spin.rs

@@ -39,7 +39,7 @@ pub fn spin<
             ),
             OutputType::<()>::new(),
         ),
-        DelayAction::new(1.0),
+        DelayAction::new(6.0),
         ActionWhile::new(TupleSecond::new(ActionConcurrent::new(
             act_nest!(
                 ActionSequence::new,