wip: fix slalom at night

Author: SarangaR

src/config/slalom.rs

@@ -1,3 +1,5 @@
+use std::ops::RangeInclusive;
+
 use super::Side;
 use serde::{Deserialize, Serialize};
 
@@ -15,6 +17,8 @@ pub struct Config {
     pub traversal_duration: f32,
     pub yaw_adjustment: f32,
     pub yaw_speed: f32,
+    pub area_bounds: RangeInclusive<f64>,
+    pub correction_factor: f32,
 }
 
 impl Default for Config {
@@ -32,6 +36,8 @@ impl Default for Config {
             traversal_duration: 6.0,
             yaw_adjustment: 15.0,
             yaw_speed: 0.2,
+            area_bounds: 1000.0..=11000.0,
+            correction_factor: 0.4,
         }
     }
 }

src/missions/slalom.rs

@@ -43,7 +43,7 @@ pub async fn slalom<
 
     let mut vision = VisionNormAngle::<Con, Slalom, f64>::new(
         context,
-        Slalom::from_color_profile(color_profile),
+        Slalom::from_color_profile(color_profile, config.area_bounds.clone()),
     );
 
     let initial_yaw = loop {
@@ -55,6 +55,12 @@ pub async fn slalom<
         }
     };
 
+    let _ = cb
+        .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth)
+        .await;
+
+    sleep(Duration::from_secs(3)).await;
+
     let mut yaw_target = 0.0;
     let mut true_count = 0;
     let mut false_count = 0;
@@ -78,20 +84,6 @@ pub async fn slalom<
     logln!("Starting slalom detection");
 
     // Default left, right if flipped
-    let _ = cb
-        .stability_1_speed_set(
-            0.0,
-            0.0,
-            if flip {
-                config.yaw_speed
-            } else {
-                -config.yaw_speed
-            },
-            0.0,
-            0.0,
-            config.depth,
-        )
-        .await;
     'detections: loop {
         #[allow(unused_variables)]
         let detections = vision.execute().await.unwrap_or_else(|e| {
@@ -132,12 +124,30 @@ pub async fn slalom<
                             logln!("true_count: {true_count}/4");
                         }
                     } else {
-                        correction = dbg!(-0.2 * x);
+                        correction = dbg!(config.correction_factor * x);
                         let _ = cb
                             .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth)
                             .await;
                     }
                 } else {
+                    #[cfg(feature = "logging")]
+                    logln!("SEARCHING");
+
+                    let _ = cb
+                        .stability_1_speed_set(
+                            0.0,
+                            0.0,
+                            if flip {
+                                config.yaw_speed
+                            } else {
+                                -config.yaw_speed
+                            },
+                            0.0,
+                            0.0,
+                            config.depth,
+                        )
+                        .await;
+
                     false_count += 1;
                     #[cfg(feature = "logging")]
                     logln!("NO DETECTIONS");
@@ -153,12 +163,28 @@ pub async fn slalom<
                 #[cfg(feature = "logging")]
                 logln!("APPROACH");
 
+                let strafe_direction = if let Left = config.side { -1.0 } else { 1.0 };
+
+                let _ = cb
+                    .stability_2_speed_set(
+                        config.speed * strafe_direction,
+                        0.0,
+                        0.0,
+                        0.0,
+                        yaw_target,
+                        config.depth,
+                    )
+                    .await;
+
+                sleep(Duration::from_secs(config.strafe_duration as u64)).await;
+
                 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, config.depth)
                     .await;
@@ -167,8 +193,13 @@ pub async fn slalom<
 
                 // init_timer.execute().await;
 
+                // let _ = cb
+                //     .stability_1_speed_set(0.0, config.speed, 0.0, 0.0, 0.0, config.depth)
+                //     .await;
+
+                //
                 let _ = cb
-                    .stability_1_speed_set(0.0, config.speed, 0.0, 0.0, 0.0, config.depth)
+                    .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, yaw_target, config.depth)
                     .await;
 
                 // traversal_timer.execute().await;

src/video_source/appsink.rs

@@ -173,7 +173,7 @@ fn pipeline_head(device_name: &str, width: u32, height: u32, framerate: u32) ->
     return format!("mfvideosrc device-index={device_name} ! image/jpeg, width={width}, height={height}, framerate={framerate}/1");
 
     #[cfg(not(target_os = "windows"))]
-    return format!("v4l2src device={device_name} exposure=30 ! image/jpeg, width={width}, height={height}, framerate={framerate}/1");
+    return format!("v4l2src device={device_name} ! image/jpeg, width={width}, height={height}, framerate={framerate}/1");
 }
 
 fn h264_enc_pipeline(bitrate: u32) -> String {

src/vision/slalom.rs

@@ -14,21 +14,34 @@ use std::ops::RangeInclusive;
 #[derive(Debug)]
 pub struct Slalom {
     color_bounds: RangeInclusive<Yuv>,
+    area_bounds: RangeInclusive<f64>,
     size: Size,
     image: MatWrapper,
 }
 
 impl Slalom {
-    pub fn new(color_bounds: RangeInclusive<Yuv>, size: Size) -> Self {
+    pub fn new(
+        color_bounds: RangeInclusive<Yuv>,
+        area_bounds: RangeInclusive<f64>,
+        size: Size,
+    ) -> Self {
         Self {
             color_bounds,
+            area_bounds,
             size,
             image: Mat::default().into(),
         }
     }
 
-    pub fn from_color_profile(color_profile: &ColorProfile) -> Self {
-        Self::new(color_profile.red.clone(), Size::from((400, 300)))
+    pub fn from_color_profile(
+        color_profile: &ColorProfile,
+        area_bounds: RangeInclusive<f64>,
+    ) -> Self {
+        Self::new(
+            color_profile.red.clone(),
+            area_bounds,
+            Size::from((400, 300)),
+        )
     }
 }
 
@@ -45,6 +58,7 @@ impl Default for Slalom {
                 u: 135,
                 v: 255,
             }),
+            1000.0..=11000.0,
             Size::from((400, 300)),
         )
     }