Path Align

Cargo.toml

@@ -23,6 +23,7 @@ required-features = ["graphing"]
 [features]
 default = []
 logging = []
+annotated_streams = []
 #unblocked_logging = ["logging"]
 cuda = ["dep:cc"]
 cuda_f16 = ["cuda"]

jetson/src/main.rs

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

playstreams.sh

@@ -0,0 +1,2 @@
+mpv rtsp://192.168.2.5:8554/front.mp4 --title=Front --no-cache --untimed  --no-demuxer-thread --vd-lavc-threads=1 --profile=low-latency --no-correct-pts --osc=no &
+mpv rtsp://192.168.2.5:8554/bottom.mp4 --title=Front --no-cache --untimed  --no-demuxer-thread --vd-lavc-threads=1 --profile=low-latency --no-correct-pts --osc=no &

src/comms/control_board/mod.rs

@@ -43,7 +43,7 @@ fn stab_2_drift() -> f32 {
             loop {
                 {
                     let mut drift_val_inner = drift_val_clone.lock().unwrap();
-                    *drift_val_inner += 0.015;
+                    // *drift_val_inner += 0.015;
                 }
                 sleep(Duration::from_secs(1)).await
             }
@@ -155,9 +155,9 @@ impl<T: 'static + AsyncWriteExt + Unpin + Send> ControlBoard<T> {
     async fn stab_tune(&self) -> Result<()> {
         self.stability_assist_pid_tune('X', 0.8, 0.0, 0.0, 0.6, false)
             .await?;
-        self.stability_assist_pid_tune('Y', 0.15, 0.0, 0.0, 0.1, false)
+        self.stability_assist_pid_tune('Y', 2.0, 0.0, 0.0, 0.1, false)
             .await?;
-        self.stability_assist_pid_tune('Z', 1.6, 1e-6, 0.0, 0.8, false)
+        self.stability_assist_pid_tune('Z', 10.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
@@ -341,7 +341,10 @@ impl<T: AsyncWrite + Unpin> ControlBoard<T> {
             y,
             target_pitch,
             target_roll,
-            (target_yaw + stab_2_drift()),
+            (
+                target_yaw
+                // + stab_2_drift()
+            ),
             target_depth,
         ]
         .iter()

src/comms/control_board/response.rs

@@ -130,7 +130,7 @@ impl ResponseMap {
                 } else if message_body.get(0..7) == Some(&BNO055D) {
                     static mut PREV_YAW_PRINT: SystemTime = SystemTime::UNIX_EPOCH;
                     let new_status = message_body[7..].try_into().unwrap();
-                    /*
+
                     let now = SystemTime::now();
                     unsafe {
                         if now.duration_since(PREV_YAW_PRINT).unwrap() > Duration::from_secs(1) {
@@ -140,7 +140,7 @@ impl ResponseMap {
                         PREV_YAW_PRINT = SystemTime::now();
                         }
                     }
-                    */
+
 
                     *bno055_status.write().await = Some(new_status);
                 } else if message_body.get(0..7) == Some(&MS5837D) {

src/main.rs

@@ -19,13 +19,13 @@ use sw8s_rust_lib::{
             buoy_circle_sequence, buoy_circle_sequence_blind, buoy_circle_sequence_model,
         },
         coinflip::coinflip,
-        example::initial_descent,
+        example::{initial_descent, pid_test},
         fancy_octagon::fancy_octagon,
         fire_torpedo::{FireLeftTorpedo, FireRightTorpedo},
         gate::{gate_run_coinflip, gate_run_complex, gate_run_naive, gate_run_testing},
         meb::WaitArm,
         octagon::octagon,
-        path_align::path_align,
+        path_align::path_align_procedural,
         reset_torpedo::ResetTorpedo,
         spin::spin,
         vision::PIPELINE_KILL,
@@ -374,14 +374,13 @@ async fn run_mission(mission: &str) -> Result<()> {
             Ok(())
         }
         "path_align" => {
-            let _ = path_align(&FullActionContext::new(
+            let _ = path_align_procedural(&FullActionContext::new(
                 control_board().await,
                 meb().await,
                 front_cam().await,
                 bottom_cam().await,
                 gate_target().await,
             ))
-            .execute()
             .await;
             Ok(())
         }
@@ -411,6 +410,18 @@ async fn run_mission(mission: &str) -> Result<()> {
             .await;
             Ok(())
         }
+        "pid_test" => {
+            let _ = pid_test(&FullActionContext::new(
+                control_board().await,
+                meb().await,
+                front_cam().await,
+                bottom_cam().await,
+                gate_target().await,
+            ))
+            .execute()
+            .await;
+            Ok(())
+        }
         "octagon" => {
             let _ = octagon(static_context().await).execute().await;
             Ok(())

src/missions/action_context.rs

@@ -1,5 +1,6 @@
 use core::fmt::Debug;
 use opencv::core::Mat;
+use opencv::mod_prelude::ToInputArray;
 use tokio::io::{AsyncWriteExt, WriteHalf};
 use tokio::sync::RwLock;
 use tokio_serial::SerialStream;
@@ -28,8 +29,10 @@ pub trait GetMainElectronicsBoard: Send + Sync {
  * Inherit this trait if you have a front camera
  */
 #[allow(async_fn_in_trait)]
-pub trait GetFrontCamMat {
+pub trait FrontCamIO {
     fn get_front_camera_mat(&self) -> impl std::future::Future<Output = Mat> + Send;
+    #[cfg(feature = "annotated_streams")]
+    async fn annotate_front_camera(&self, image: &impl ToInputArray);
     async fn get_desired_buoy_gate(&self) -> Target;
     async fn set_desired_buoy_gate(&mut self, value: Target) -> &Self;
 }
@@ -38,8 +41,10 @@ pub trait GetFrontCamMat {
  * Inherit this trait if you have a bottom camera
  */
 #[allow(async_fn_in_trait)]
-pub trait GetBottomCamMat {
+pub trait BottomCamIO {
     async fn get_bottom_camera_mat(&self) -> Mat;
+    #[cfg(feature = "annotated_streams")]
+    async fn annotate_bottom_camera(&self, image: &impl ToInputArray);
 }
 
 /*
@@ -93,10 +98,14 @@ impl GetMainElectronicsBoard for FullActionContext<'_, WriteHalf<SerialStream>>
     }
 }
 
-impl<T: AsyncWriteExt + Unpin + Send> GetFrontCamMat for FullActionContext<'_, T> {
+impl<T: AsyncWriteExt + Unpin + Send> FrontCamIO for FullActionContext<'_, T> {
     async fn get_front_camera_mat(&self) -> Mat {
         self.front_cam.get_mat().await
     }
+    #[cfg(feature = "annotated_streams")]
+    async fn annotate_front_camera(&self, image: &impl ToInputArray) {
+        self.front_cam.push_annotated_frame(image);
+    }
     async fn get_desired_buoy_gate(&self) -> Target {
         let res = self.desired_buoy_target.read().await;
         (*res).clone()
@@ -107,10 +116,14 @@ impl<T: AsyncWriteExt + Unpin + Send> GetFrontCamMat for FullActionContext<'_, T
     }
 }
 
-impl<T: AsyncWriteExt + Unpin + Send> GetBottomCamMat for FullActionContext<'_, T> {
+impl<T: AsyncWriteExt + Unpin + Send> BottomCamIO for FullActionContext<'_, T> {
     async fn get_bottom_camera_mat(&self) -> Mat {
         self.bottom_cam.get_mat().await
     }
+    #[cfg(feature = "annotated_streams")]
+    async fn annotate_bottom_camera(&self, image: &impl ToInputArray) {
+        self.bottom_cam.push_annotated_frame(image);
+    }
 }
 
 impl GetControlBoard<WriteHalf<SerialStream>> for EmptyActionContext {
@@ -125,10 +138,14 @@ impl GetMainElectronicsBoard for EmptyActionContext {
     }
 }
 
-impl GetFrontCamMat for EmptyActionContext {
+impl FrontCamIO for EmptyActionContext {
     async fn get_front_camera_mat(&self) -> Mat {
         todo!()
     }
+    #[cfg(feature = "annotated_streams")]
+    async fn annotate_front_camera(&self, image: &impl ToInputArray) {
+        todo!();
+    }
     async fn get_desired_buoy_gate(&self) -> Target {
         todo!()
     }
@@ -137,8 +154,12 @@ impl GetFrontCamMat for EmptyActionContext {
     }
 }
 
-impl GetBottomCamMat for EmptyActionContext {
+impl BottomCamIO for EmptyActionContext {
     async fn get_bottom_camera_mat(&self) -> Mat {
         todo!()
     }
+    #[cfg(feature = "annotated_streams")]
+    async fn annotate_bottom_camera(&self, image: &impl ToInputArray) {
+        todo!();
+    }
 }

src/missions/align_buoy.rs

@@ -30,15 +30,15 @@ use crate::{
 
 use super::{
     action::ActionExec,
-    action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard},
+    action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard},
 };
 
 pub fn buoy_align<
     Con: Send
         + Sync
         + GetControlBoard<WriteHalf<SerialStream>>
         + GetMainElectronicsBoard
-        + GetFrontCamMat
+        + FrontCamIO
         + Unpin,
 >(
     context: &'static Con,
@@ -149,7 +149,7 @@ pub fn buoy_align_shot<
         + Sync
         + GetControlBoard<WriteHalf<SerialStream>>
         + GetMainElectronicsBoard
-        + GetFrontCamMat
+        + FrontCamIO
         + Unpin,
 >(
     context: &'static Con,

src/missions/buoy_hit.rs

@@ -1,6 +1,6 @@
 use super::{
     action::{Action, ActionExec, ActionSequence, ActionWhile},
-    action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard},
+    action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard},
     basic::DelayAction,
     movement::{StraightMovement, ZeroMovement},
 };
@@ -54,7 +54,7 @@ impl<T> Action for FindBuoy<'_, T> {}
 
 impl<T> ActionExec<Result<()>> for FindBuoy<'_, T>
 where
-    T: GetControlBoard<WriteHalf<SerialStream>> + GetFrontCamMat + Sync + Unpin,
+    T: GetControlBoard<WriteHalf<SerialStream>> + FrontCamIO + Sync + Unpin,
 {
     async fn execute(&mut self) -> Result<()> {
         let camera_aquisition = self.context.get_front_camera_mat();
@@ -75,7 +75,7 @@ where
 }
 impl<T> ActionExec<Result<()>> for DriveToBuoyVision<'_, T>
 where
-    T: GetControlBoard<WriteHalf<SerialStream>> + GetFrontCamMat + Sync + Unpin,
+    T: GetControlBoard<WriteHalf<SerialStream>> + FrontCamIO + Sync + Unpin,
 {
     async fn execute(&mut self) -> Result<()> {
         let camera_aquisition = self.context.get_front_camera_mat();
@@ -121,7 +121,7 @@ pub fn buoy_collision_sequence<
         + Sync
         + GetControlBoard<WriteHalf<SerialStream>>
         + GetMainElectronicsBoard
-        + GetFrontCamMat
+        + FrontCamIO
         + Unpin,
     T: Send + Sync,
 >(

src/missions/circle_buoy.rs

@@ -21,7 +21,7 @@ use crate::{
 
 use super::{
     action::{ActionExec, ActionSequence},
-    action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard},
+    action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard},
     basic::DelayAction,
     movement::ZeroMovement,
 };
@@ -35,7 +35,7 @@ pub fn buoy_circle_sequence<
         + Sync
         + GetControlBoard<WriteHalf<SerialStream>>
         + GetMainElectronicsBoard
-        + GetFrontCamMat
+        + FrontCamIO
         + Unpin,
 >(
     context: &Con,
@@ -95,7 +95,7 @@ pub fn buoy_circle_sequence_model<
         + Sync
         + GetControlBoard<WriteHalf<SerialStream>>
         + GetMainElectronicsBoard
-        + GetFrontCamMat
+        + FrontCamIO
         + Unpin,
 >(
     context: &'static Con,
@@ -144,7 +144,7 @@ pub fn buoy_circle_sequence_blind<
         + Sync
         + GetControlBoard<WriteHalf<SerialStream>>
         + GetMainElectronicsBoard
-        + GetFrontCamMat
+        + FrontCamIO
         + Unpin,
 >(
     context: &'static Con,

src/missions/coinflip.rs

@@ -20,7 +20,7 @@ use super::{
         wrap_action, ActionChain, ActionConcurrent, ActionExec, ActionSequence, ActionWhile,
         FirstValid,
     },
-    action_context::{GetControlBoard, GetFrontCamMat, GetMainElectronicsBoard},
+    action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard},
     basic::DelayAction,
     comms::StartBno055,
     extra::{CountTrue, OutputType},
@@ -29,21 +29,17 @@ use super::{
 };
 
 pub fn coinflip<
-    Con: Send
-        + Sync
-        + GetControlBoard<WriteHalf<SerialStream>>
-        + GetMainElectronicsBoard
-        + GetFrontCamMat,
+    Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
 >(
     context: &Con,
 ) -> impl ActionExec<()> + '_ {
     const TRUE_COUNT: u32 = 4;
     const DELAY_TIME: f32 = 3.0;
 
-    const DEPTH: f32 = -1.15;
+    const DEPTH: f32 = -1.6;
     const ALIGN_X_SPEED: f32 = 0.0;
     const ALIGN_Y_SPEED: f32 = 0.0;
-    const ALIGN_YAW_SPEED: f32 = -6.0;
+    const ALIGN_YAW_SPEED: f32 = -3.0;
     const ALIGN_YAW_CORRECTION_SPEED: f32 = 0.0;
 
     act_nest!(