pre semis

Author: Cowboylaserkittenjetshark

jetson/src/main.rs

@@ -39,7 +39,7 @@ async fn main() -> Result<()> {
             "build".to_string(),
             "--release".to_string(),
             "--features".to_string(),
-            "logging,annotated_streams".to_string(),
+            "logging".to_string(),
         ];
     }
 

src/comms/control_board/mod.rs

@@ -129,7 +129,7 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
             .await?;
         self.stability_assist_pid_tune('Y', 2.0, 0.0, 0.0, 0.1, false)
             .await?;
-        self.stability_assist_pid_tune('Z', 6.0, 0.0, 0.0, 1.0, false)
+        self.stability_assist_pid_tune('Z', 4.0, 0.0, 0.0, 1.0, false)
             .await?;
         self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false)
             .await

src/main.rs

@@ -24,8 +24,8 @@ use sw8s_rust_lib::{
         example::{initial_descent, pid_test},
         fire_torpedo::{FireLeftTorpedo, FireRightTorpedo},
         gate::{
-            gate_run_complex, gate_run_cv_procedural, gate_run_naive, gate_run_procedural,
-            gate_run_testing,
+            gate_run_complex, gate_run_cv_procedural, gate_run_dead_reckon, gate_run_naive,
+            gate_run_procedural, gate_run_testing,
         },
         meb::WaitArm,
         octagon::octagon,
@@ -399,6 +399,27 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> {
             &config.missions.gate,
             &config.get_color_profile().unwrap(),
         )),
+        "gate_run_yolo" => ctwrap!(gate_run_procedural(
+            &FullActionContext::new(
+                control_board().await,
+                meb().await,
+                front_cam().await,
+                bottom_cam().await,
+                gate_target().await,
+            ),
+            &config.missions.gate
+        )),
+        "gate_run_reckon" => ctwrap!(gate_run_dead_reckon(
+            &FullActionContext::new(
+                control_board().await,
+                meb().await,
+                front_cam().await,
+                bottom_cam().await,
+                gate_target().await,
+            ),
+            &config.missions.gate,
+            &config.get_color_profile().unwrap(),
+        )),
         "gate_run_testing" => ctwrap!(gate_run_testing(&FullActionContext::new(
             control_board().await,
             meb().await,

src/missions/gate.rs

@@ -39,6 +39,63 @@ use super::{
     vision::{DetectTarget, ExtractPosition, VisionNorm, VisionNormOffset},
 };
 
+pub async fn gate_run_dead_reckon<
+    Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
+>(
+    context: &Con,
+    config: &Config,
+    color_profile: &ColorProfile,
+) {
+    #[cfg(feature = "logging")]
+    logln!("Starting Procedural Gate");
+
+    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 _ = cb
+    //     .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth)
+    //     .await;
+
+    // sleep(Duration::from_secs_f32(config.init_duration)).await;
+    // let mut mult = 1.0;
+    // if let Side::Left = config.side {
+    //     mult = -1.0;
+    // } else {
+    //     mult = 1.0;
+    // }
+    // let _ = cb
+    //     .stability_2_speed_set(
+    //         config.strafe_speed * mult,
+    //         0.0,
+    //         0.0,
+    //         0.0,
+    //         initial_yaw,
+    //         config.depth,
+    //     )
+    //     .await;
+
+    // sleep(Duration::from_secs_f32(config.strafe_duration)).await;
+
+    let _ = cb
+        .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, initial_yaw, config.depth)
+        .await;
+
+    sleep(Duration::from_secs_f32(config.traversal_duration)).await;
+
+    let _ = cb
+        .stability_1_speed_set(0.0, 0.0, 0.0, 0.0, 0.0, config.depth)
+        .await;
+}
+
 pub async fn gate_run_cv_procedural<
     Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
 >(

src/missions/slalom.rs

@@ -151,7 +151,7 @@ pub async fn slalom<
                     false_count += 1;
                     #[cfg(feature = "logging")]
                     logln!("NO DETECTIONS");
-                    if false_count >= 100 {
+                    if false_count >= 500 {
                         #[cfg(feature = "logging")]
                         logln!("KILLED NO DET");
                         break 'detections;

src/missions/spin.rs

@@ -24,10 +24,10 @@ pub fn spin<
 >(
     context: &Con,
 ) -> impl ActionExec<()> + '_ {
-    const GATE_DEPTH: f32 = -2.0;
-    const DEPTH: f32 = -2.0;
+    const GATE_DEPTH: f32 = -1.75;
+    const DEPTH: f32 = -1.15;
     const Z_TARGET: f32 = 0.0;
-    const FORWARD_SPEED: f32 = 1.0;
+    const FORWARD_SPEED: f32 = 0.0;
     const SPIN_SPEED: f32 = 1.0;
 
     act_nest!(
@@ -39,7 +39,7 @@ pub fn spin<
             ),
             OutputType::<()>::new(),
         ),
-        DelayAction::new(6.0),
+        DelayAction::new(2.0),
         ActionWhile::new(TupleSecond::new(ActionConcurrent::new(
             act_nest!(
                 ActionSequence::new,

src/video_source/appsink.rs

@@ -76,6 +76,7 @@ impl Camera {
             + " rtspclientsink location=rtsp://127.0.0.1:8554/"
             + camera_name
             + "_annotated.mp4 ";
+        #[cfg(feature = "annotated_streams")]
         dbg!(&output_string);
         // pipeline_head(camera_path, camera_dimensions.0, camera_dimensions.1, 30)
         // "appsrc ! image/jpeg, width=480, height=640, framerate=30/1".to_string()