wip: working slalom align

Author: Cowboylaserkittenjetshark

src/missions/slalom.rs

@@ -112,12 +112,16 @@ pub async fn slalom<
                 logln!("ALIGN");
 
                 if let Some(position) = positions.next() {
+                    false_count = 0;
                     let x = *position.x() as f32;
+                    dbg!(&x);
                     let mut correction = 0.0;
                     if x.abs() < 0.2 {
                         true_count += 1;
                         if true_count >= 4 {
                             correction = 0.0;
+                            #[cfg(feature = "logging")]
+                            logln!("ALIGNED");
                             slalom_state = SlalomState::Approach;
                             if let Some(current_angle) = cb.responses().get_angles().await {
                                 let current_yaw = *current_angle.yaw();
@@ -128,14 +132,18 @@ pub async fn slalom<
                             logln!("true_count: {true_count}/4");
                         }
                     } else {
-                        correction = 0.5 * x;
+                        correction = dbg!(-0.2 * x);
                         let _ = cb
                             .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth)
                             .await;
                     }
                 } else {
                     false_count += 1;
+                    #[cfg(feature = "logging")]
+                    logln!("NO DETECTIONS");
                     if false_count >= 100 {
+                        #[cfg(feature = "logging")]
+                        logln!("KILLED NO DET");
                         break 'detections;
                     }
                 }
@@ -145,29 +153,27 @@ pub async fn slalom<
                 #[cfg(feature = "logging")]
                 logln!("APPROACH");
 
+                yaw_target = (yaw_target
+                    + (if let Left = config.side {
+                        config.yaw_adjustment
+                    } else {
+                        -config.yaw_adjustment
+                    }));
                 let _ = cb
-                    .stability_2_speed_set(
-                        0.0,
-                        0.0,
-                        0.0,
-                        0.0,
-                        (yaw_target
-                            + (if let Left = config.side {
-                                -config.yaw_adjustment
-                            } else {
-                                config.yaw_adjustment
-                            })) as f32,
-                        config.depth,
-                    )
+                    .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, yaw_target, config.depth)
                     .await;
 
-                init_timer.execute().await;
+                sleep(Duration::from_secs(config.init_duration as u64)).await;
+
+                // init_timer.execute().await;
 
                 let _ = cb
-                    .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, 0.0, config.depth)
+                    .stability_1_speed_set(0.0, config.speed, 0.0, 0.0, 0.0, config.depth)
                     .await;
 
-                traversal_timer.execute().await;
+                // traversal_timer.execute().await;
+                sleep(Duration::from_secs(config.traversal_duration as u64)).await;
+
                 break 'detections;
             }
         }

src/vision/slalom.rs

@@ -5,7 +5,7 @@ use opencv::{
     core::{in_range, Point, Scalar, Size, Vector},
     imgproc::{
         box_points, contour_area_def, cvt_color_def, find_contours_def, min_area_rect,
-        CHAIN_APPROX_SIMPLE, COLOR_BGR2HSV, RETR_EXTERNAL,
+        CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, RETR_EXTERNAL,
     },
     prelude::{Mat, MatTraitConst, MatTraitConstManual},
 };
@@ -64,7 +64,7 @@ impl VisualDetector<f64> for Slalom {
         self.image = resize(input_image, &self.size)?.into();
         let mut yuv_image = Mat::default();
 
-        cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2HSV)?;
+        cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?;
 
         let color_start = self.color_bounds.start();
         let color_end = self.color_bounds.end();