diff --git a/src/config.rs b/src/config.rs deleted file mode 100644 index dce3dbf4ab4..00000000000 --- a/src/config.rs +++ /dev/null @@ -1,71 +0,0 @@ -use std::{ - fs::read_to_string, - fs::write, - ops::{Deref, DerefMut}, -}; - -use serde::{Deserialize, Serialize}; - -#[derive(Debug, Serialize, Deserialize)] -pub struct ConfigFile { - pub control_board_path: String, - pub control_board_backup_path: String, - pub meb_path: String, - pub front_cam: String, - pub bottom_cam: String, - pub standard_depth: f32, -} - -impl Default for ConfigFile { - fn default() -> Self { - Self { - control_board_path: "/dev/ttyACM0".to_string(), - control_board_backup_path: "/dev/ttyACM3".to_string(), - meb_path: "/dev/ttyACM2".to_string(), - front_cam: "/dev/video1".to_string(), - bottom_cam: "/dev/video0".to_string(), - standard_depth: 1.0, - } - } -} - -const CONFIG_FILE: &str = "config.toml"; - -#[derive(Debug)] -pub struct Configuration { - inner: ConfigFile, -} - -impl Default for Configuration { - fn default() -> Self { - let inner = if let Ok(config_string) = read_to_string(CONFIG_FILE) { - match toml::from_str(&config_string) { - Ok(x) => x, - //Err(x) => panic!("Config file parsing: {:#?}", x), - Err(_) => ConfigFile::default(), - } - } else { - ConfigFile::default() - }; - Self { inner } - } -} - -impl Drop for Configuration { - fn drop(&mut self) { - write(CONFIG_FILE, toml::to_string(&self.inner).unwrap()).unwrap(); - } -} - -impl Deref for Configuration { - type Target = ConfigFile; - fn deref(&self) -> &Self::Target { - &self.inner - } -} - -impl DerefMut for Configuration { - fn deref_mut(&mut self) -> &mut Self::Target { - &mut self.inner - } -} diff --git a/src/config/gate.rs b/src/config/gate.rs new file mode 100644 index 00000000000..cb9f5404a8f --- /dev/null +++ b/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, + } + } +} diff --git a/src/config/mod.rs b/src/config/mod.rs new file mode 100644 index 00000000000..04274b74cba --- /dev/null +++ b/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 { + 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, +} diff --git a/src/config/path_align.rs b/src/config/path_align.rs new file mode 100644 index 00000000000..0375be3d07e --- /dev/null +++ b/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, + } + } +} diff --git a/src/lib.rs b/src/lib.rs index 62f95309045..879033567d3 100644 --- a/src/lib.rs +++ b/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; diff --git a/src/main.rs b/src/main.rs index 57efea00fb3..c4704169ccc 100644 --- a/src/main.rs +++ b/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 = 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>> = OnceCell::const_new(); async fn control_board() -> &'static ControlBoard> { + 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> { MEB_CELL .get_or_init(|| async { MainElectronicsBoard::>::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 UnboundedSender { } 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), ) diff --git a/src/missions/action_context.rs b/src/missions/action_context.rs index 51f4ad73ec5..3498b12e19a 100644 --- a/src/missions/action_context.rs +++ b/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; diff --git a/src/missions/gate.rs b/src/missions/gate.rs index bc57f699c7d..a765bcc5b04 100644 --- a/src/missions/gate.rs +++ b/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> + GetMainElectronicsBoard + FrontCamIO, >( - context: &Con, -) -> impl ActionExec> + '_ { + context: &'a Con, + config: &Config, +) -> impl ActionExec> + '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::, Offset2D>::new(Target::Red), DetectTarget::, Offset2D>::new(Target::Pole), ), - CountFalse::new(1), + CountFalse::new(config.false_count), )), ActionChain::new( Stability2Movement::new( diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index a59b543809c..0527fea25a9 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -2,6 +2,7 @@ use tokio::io::WriteHalf; use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; +use crate::config::path_align::Config; use crate::{act_nest, missions::vision::VisionNormBottomAngle, vision::path_cv::PathCV}; use super::{ @@ -13,11 +14,8 @@ pub async fn path_align_procedural< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, >( context: &Con, + config: &Config, ) { - const DEPTH: f32 = -1.6; - const PATH_ALIGN_SPEED: f32 = 0.3; - const DETECTIONS: u8 = 10; - #[cfg(feature = "logging")] logln!("Starting path align"); @@ -36,11 +34,9 @@ pub async fn path_align_procedural< }; let _ = cb - .stability_2_speed_set(0.0, PATH_ALIGN_SPEED, 0.0, 0.0, initial_yaw, DEPTH) + .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, initial_yaw, config.depth) .await; - // sleep(Duration::from_secs(10)).await; - let mut last_set_yaw = initial_yaw; let mut consec_detections = 0; @@ -48,7 +44,7 @@ pub async fn path_align_procedural< logln!("Starting path detection"); loop { - if consec_detections >= DETECTIONS { + if consec_detections >= config.detections { #[cfg(feature = "logging")] logln!("Finished path align"); @@ -88,7 +84,7 @@ pub async fn path_align_procedural< } if let Err(e) = cb - .stability_2_speed_set(x, y, 0.0, 0.0, last_set_yaw, DEPTH) + .stability_2_speed_set(x, y, 0.0, 0.0, last_set_yaw, config.depth) .await { #[cfg(feature = "logging")] @@ -102,7 +98,7 @@ pub async fn path_align_procedural< #[cfg(feature = "logging")] logln!("Positive detection count: {consec_detections}"); } - cb.stability_2_speed_set(0.0, 1.0, 0.0, 0.0, last_set_yaw, DEPTH) + cb.stability_2_speed_set(0.0, 1.0, 0.0, 0.0, last_set_yaw, config.depth) .await; sleep(Duration::from_secs(1)).await; }