Mission config

src/config/gate.rs

@@ -0,0 +1,20 @@
+use serde::{Deserialize, Serialize};
+
+#[derive(Debug, Serialize, Deserialize)]
+pub struct Config {
+    pub depth: f32,
+    pub speed: f32,
+    pub true_count: u32,
+    pub false_count: u32,
+}
+
+impl Default for Config {
+    fn default() -> Self {
+        Self {
+            depth: -1.25,
+            speed: 0.2,
+            true_count: 4,
+            false_count: 1,
+        }
+    }
+}

src/config/mod.rs

@@ -0,0 +1,56 @@
+pub mod gate;
+pub mod path_align;
+
+use std::{
+    fs::{read_to_string, write},
+    ops::{Deref, DerefMut},
+    path::PathBuf,
+};
+
+use anyhow::Result;
+use crossbeam::epoch::CompareAndSetOrdering;
+use serde::{Deserialize, Serialize};
+
+// Default values
+const CONFIG_FILE: &str = "config.toml";
+const CONTROL_BOARD_PATH: &str = "/dev/ttyACM0";
+const CONTROL_BOARD_BACKUP_PATH: &str = "/dev/ttyACM3";
+const MEB_PATH: &str = "/dev/ttyACM2";
+const FRONT_CAM: &str = "/dev/video0";
+const BOTTOM_CAM: &str = "/dev/video1";
+
+#[derive(Debug, Serialize, Deserialize)]
+pub struct Config {
+    pub control_board_path: String,
+    pub control_board_backup_path: String,
+    pub meb_path: String,
+    pub front_cam_path: String,
+    pub bottom_cam_path: String,
+    pub missions: Missions,
+}
+
+impl Config {
+    pub fn new() -> Result<Self> {
+        let config_string = read_to_string(CONFIG_FILE)?;
+        Ok(toml::from_str(&config_string)?)
+    }
+}
+
+impl Default for Config {
+    fn default() -> Self {
+        Self {
+            control_board_path: CONTROL_BOARD_PATH.to_string(),
+            control_board_backup_path: CONTROL_BOARD_BACKUP_PATH.to_string(),
+            meb_path: MEB_PATH.to_string(),
+            front_cam_path: FRONT_CAM.to_string(),
+            bottom_cam_path: BOTTOM_CAM.to_string(),
+            missions: Missions::default(),
+        }
+    }
+}
+
+#[derive(Debug, Default, Serialize, Deserialize)]
+pub struct Missions {
+    pub gate: gate::Config,
+    pub path_align: path_align::Config,
+}

src/config/path_align.rs

@@ -0,0 +1,18 @@
+use serde::{Deserialize, Serialize};
+
+#[derive(Debug, Serialize, Deserialize)]
+pub struct Config {
+    pub depth: f32,
+    pub speed: f32,
+    pub detections: u8,
+}
+
+impl Default for Config {
+    fn default() -> Self {
+        Self {
+            depth: -1.25,
+            speed: 0.3,
+            detections: 10,
+        }
+    }
+}

src/lib.rs

@@ -35,6 +35,7 @@ macro_rules! logln {
 pub const POOL_YAW_SIGN: f32 = -1.0;
 
 pub mod comms;
+pub mod config;
 pub mod missions;
 pub mod video_source;
 pub mod vision;

src/main.rs

@@ -1,5 +1,5 @@
 use anyhow::{bail, Result};
-use config::Configuration;
+use crossbeam::epoch::Pointable;
 use std::env::temp_dir;
 
 use std::env;
@@ -9,6 +9,7 @@ use sw8s_rust_lib::{
         control_board::{ControlBoard, SensorStatuses},
         meb::MainElectronicsBoard,
     },
+    config::Config,
     logln,
     missions::{
         action::ActionExec,
@@ -47,21 +48,34 @@ use tokio_serial::SerialStream;
 pub mod config;
 use std::time::Duration;
 
+static CONFIG_CELL: OnceCell<Config> = OnceCell::const_new();
+async fn config() -> &'static Config {
+    CONFIG_CELL
+        .get_or_init(|| async {
+            Config::new().unwrap_or_else(|e| {
+                logln!("Error getting config file: {:#?}\nUsing default config", e);
+                Config::default()
+            })
+        })
+        .await
+}
+
 static CONTROL_BOARD_CELL: OnceCell<ControlBoard<WriteHalf<SerialStream>>> = OnceCell::const_new();
 async fn control_board() -> &'static ControlBoard<WriteHalf<SerialStream>> {
+    let config = config().await;
     CONTROL_BOARD_CELL
         .get_or_init(|| async {
-            let board = ControlBoard::serial(&Configuration::default().control_board_path).await;
+            let board = ControlBoard::serial(config.control_board_path.as_str()).await;
             match board {
                 Ok(x) => x,
                 Err(e) => {
                     logln!("Error initializing control board: {:#?}", e);
                     let backup_board =
-                        ControlBoard::serial(&Configuration::default().control_board_backup_path)
+                        ControlBoard::serial(config.control_board_backup_path.as_str())
                             .await
                             .unwrap();
                     backup_board.reset().await.unwrap();
-                    ControlBoard::serial(&Configuration::default().control_board_path)
+                    ControlBoard::serial(config.control_board_path.as_str())
                         .await
                         .unwrap()
                 }
@@ -75,7 +89,7 @@ async fn meb() -> &'static MainElectronicsBoard<WriteHalf<SerialStream>> {
     MEB_CELL
         .get_or_init(|| async {
             MainElectronicsBoard::<WriteHalf<SerialStream>>::serial(
-                &Configuration::default().meb_path,
+                config().await.meb_path.as_str(),
             )
             .await
             .unwrap()
@@ -88,7 +102,7 @@ async fn front_cam() -> &'static Camera {
     FRONT_CAM_CELL
         .get_or_init(|| async {
             Camera::jetson_new(
-                &Configuration::default().front_cam,
+                config().await.front_cam_path.as_str(),
                 "front",
                 &temp_dir().join("cams_".to_string() + &TIMESTAMP),
             )
@@ -102,7 +116,7 @@ async fn bottom_cam() -> &'static Camera {
     BOTTOM_CAM_CELL
         .get_or_init(|| async {
             Camera::jetson_new(
-                &Configuration::default().bottom_cam,
+                config().await.bottom_cam_path.as_str(),
                 "bottom",
                 &temp_dir().join("cams_".to_string() + &TIMESTAMP),
             )
@@ -136,7 +150,6 @@ async fn static_context() -> &'static FullActionContext<'static, WriteHalf<Seria
 #[tokio::main]
 async fn main() {
     let shutdown_tx = shutdown_handler().await;
-    let _config = Configuration::default();
 
     let orig_hook = std::panic::take_hook();
     std::panic::set_hook(Box::new(move |panic_info| {
@@ -212,6 +225,7 @@ async fn shutdown_handler() -> UnboundedSender<i32> {
 }
 
 async fn run_mission(mission: &str) -> Result<()> {
+    let config = config().await;
     let res = match mission.to_lowercase().as_str() {
         "arm" => {
             WaitArm::new(static_context().await).execute().await;
@@ -342,13 +356,16 @@ async fn run_mission(mission: &str) -> Result<()> {
             Ok(())
         }
         "gate_run_coinflip" => {
-            let _ = gate_run_coinflip(&FullActionContext::new(
-                control_board().await,
-                meb().await,
-                front_cam().await,
-                bottom_cam().await,
-                gate_target().await,
-            ))
+            let _ = gate_run_coinflip(
+                &FullActionContext::new(
+                    control_board().await,
+                    meb().await,
+                    front_cam().await,
+                    bottom_cam().await,
+                    gate_target().await,
+                ),
+                &config.missions.gate,
+            )
             .execute()
             .await;
             Ok(())
@@ -374,13 +391,16 @@ async fn run_mission(mission: &str) -> Result<()> {
             Ok(())
         }
         "path_align" => {
-            let _ = path_align_procedural(&FullActionContext::new(
-                control_board().await,
-                meb().await,
-                front_cam().await,
-                bottom_cam().await,
-                gate_target().await,
-            ))
+            let _ = path_align_procedural(
+                &FullActionContext::new(
+                    control_board().await,
+                    meb().await,
+                    front_cam().await,
+                    bottom_cam().await,
+                    gate_target().await,
+                ),
+                &config.missions.path_align,
+            )
             .await;
             Ok(())
         }
@@ -484,7 +504,7 @@ async fn run_mission(mission: &str) -> Result<()> {
         },
         "open_cam_test" => {
             Camera::jetson_new(
-                &Configuration::default().bottom_cam,
+                config.bottom_cam_path.as_str(),
                 "front",
                 &temp_dir().join("cams_".to_string() + &TIMESTAMP),
             )

src/missions/action_context.rs

@@ -47,12 +47,6 @@ pub trait BottomCamIO {
     async fn annotate_bottom_camera(&self, image: &impl ToInputArray);
 }
 
-/*
-pub trait GetConfig {
-    async fn get_config(&self) -> Configuration;
-}
-*/
-
 #[derive(Debug)]
 pub struct EmptyActionContext;
 

src/missions/gate.rs

@@ -3,6 +3,7 @@ use tokio_serial::SerialStream;
 
 use crate::{
     act_nest,
+    config::gate::Config,
     missions::{
         action::{ActionConcurrentSplit, ActionDataConditional},
         basic::descend_depth_and_go_forward,
@@ -103,13 +104,15 @@ pub fn gate_run_complex<
 }
 
 pub fn gate_run_coinflip<
+    'a,
     Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
 >(
-    context: &Con,
-) -> impl ActionExec<anyhow::Result<()>> + '_ {
+    context: &'a Con,
+    config: &Config,
+) -> impl ActionExec<anyhow::Result<()>> + 'a {
     const TIMEOUT: f32 = 30.0;
 
-    let depth: f32 = -1.75;
+    let depth = config.depth;
 
     act_nest!(
         ActionSequence::new,
@@ -125,7 +128,7 @@ pub fn gate_run_coinflip<
         ),
         act_nest!(
             ActionSequence::new,
-            adjust_logic(context, depth, CountTrue::new(4)),
+            adjust_logic(context, depth, CountTrue::new(config.true_count)),
             // adjust_logic(context, depth, CountFalse::new(10)),
             ActionChain::new(
                 Stability2Movement::new(
@@ -144,7 +147,7 @@ pub fn gate_run_coinflip<
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::Red),
                     DetectTarget::<Target, YoloClass<Target>, Offset2D<f64>>::new(Target::Pole),
                 ),
-                CountFalse::new(1),
+                CountFalse::new(config.false_count),
             )),
             ActionChain::new(
                 Stability2Movement::new(