From e269792e32b186a94c7e0a374a4002e2c7a20a0e Mon Sep 17 00:00:00 2001 From: Michael Kamprath Date: Fri, 25 Oct 2024 14:33:27 -0700 Subject: [PATCH] updated millis count to u64 --- Cargo.toml | 2 +- src/driver.rs | 4 +-- src/model/heading.rs | 20 +++++++------- src/model/pid_controller.rs | 6 ++--- src/robot.rs | 12 +++++---- src/robot/debouncer.rs | 6 ++--- src/robot/file_storage.rs | 33 +++++++++++------------- src/robot/telemetry/straight_movement.rs | 4 +-- src/system/millis.rs | 22 +++++++++++----- 9 files changed, 58 insertions(+), 51 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index c10373e..9f44087 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -26,7 +26,7 @@ mpu6050 = { git = "https://github.com/michaelkamprath/mpu6050.git", branch = "em "defmt", ] } embedded-alloc = "0.6" -i2c-character-display = { version = "0.1", features = ["defmt"] } +i2c-character-display = { version = "0.2", features = ["defmt"] } ini_core = "0.2" # We're using a Pico by default on this template diff --git a/src/driver.rs b/src/driver.rs index f3af065..c9f7022 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -19,7 +19,7 @@ use micromath::F32Ext; const PATHS_DIR: &str = "paths"; const PATH_EXTENTION: &str = "PTH"; -const SELECT_PATH_TIMEOUT_MS: u32 = 3500; +const SELECT_PATH_TIMEOUT_MS: u64 = 3500; pub struct Driver< 'a, @@ -75,7 +75,7 @@ impl< pub fn delay(&mut self, ms: u32) { // we are going to call the loop handler in the delay function every ms until the delay is complete let start_millis = millis(); - while millis() - start_millis < ms { + while millis() - start_millis < ms as u64 { self.robot.handle_loop(); self.delay.delay_us(500); } diff --git a/src/model/heading.rs b/src/model/heading.rs index fa173dc..009a41c 100644 --- a/src/model/heading.rs +++ b/src/model/heading.rs @@ -1,6 +1,6 @@ #![allow(non_camel_case_types)] -use crate::system::millis::millis; +use crate::system::millis::millis_tenths; use defmt::{error, info}; use embedded_hal::{delay::DelayNs, i2c::I2c}; @@ -13,13 +13,13 @@ use mpu6050::Mpu6050; // [-2241,-2240] --> [-1,15] [1803,1804] --> [-10,6] [1573,1574] --> [16371,16394] [98,99] --> [-2,1] [44,45] --> [0,5] [19,20] --> [0,4] // [-2239,-2238] --> [-3,16] [1804,1804] --> [0,5] [1575,1576] --> [16378,16395] [98,99] --> [-1,2] [43,44] --> [-2,1] [19,20] --> [-1,2] -const HEADING_UPDATE_INTERVAL_MS: u32 = 1; +const HEADING_UPDATE_INTERVAL_MS_TENTHS: u64 = 2; pub struct HeadingCalculator { heading: f32, gyro: Mpu6050, last_update_rate: f32, - last_update_millis: u32, + last_update_millis_tenths: u64, inited: bool, } @@ -66,7 +66,7 @@ where heading: 0.0, gyro, last_update_rate: 0.0, - last_update_millis: millis(), + last_update_millis_tenths: millis_tenths(), inited, } } @@ -106,21 +106,21 @@ where pub fn reset(&mut self) { self.heading = 0.0; self.last_update_rate = 0.0; - self.last_update_millis = millis(); + self.last_update_millis_tenths = millis_tenths(); } pub fn update(&mut self) -> f32 { if !self.is_inited() { return 0.0; } - let now = millis(); - let delta_time = now - self.last_update_millis; - if delta_time >= HEADING_UPDATE_INTERVAL_MS { + let now = millis_tenths(); + let delta_time = now - self.last_update_millis_tenths; + if delta_time >= HEADING_UPDATE_INTERVAL_MS_TENTHS { match self.gyro.get_gyro_deg() { Ok(gyro) => { - self.last_update_millis = now; + self.last_update_millis_tenths = now; // the heding is about the sensor's Z-axis - let delta_degs = gyro.z * (delta_time as f32 / 1000.0); + let delta_degs = gyro.z * (delta_time as f32 / 10000.0); self.heading += delta_degs; self.last_update_rate = gyro.z; diff --git a/src/model/pid_controller.rs b/src/model/pid_controller.rs index e372fb9..2ccfb2f 100644 --- a/src/model/pid_controller.rs +++ b/src/model/pid_controller.rs @@ -10,7 +10,7 @@ pub struct PIDController { pub setpoint: f32, pub integral: f32, pub last_error: f32, - pub last_time: u32, + pub last_time: u64, pub max_control_signal: f32, } @@ -46,7 +46,7 @@ impl PIDController { } /// Update the controller with a new measurement and the time of the measurement. - pub fn update(&mut self, measurement: f32, measurement_time: u32) -> f32 { + pub fn update(&mut self, measurement: f32, measurement_time: u64) -> f32 { if self.last_time > measurement_time { error!( "Time went backwards! {} -> {}", @@ -72,7 +72,7 @@ impl PIDController { } /// Reset the controller to its initial state. - pub fn reset(&mut self, start_time: u32) { + pub fn reset(&mut self, start_time: u64) { self.integral = 0.0; self.last_error = 0.0; self.last_time = start_time; diff --git a/src/robot.rs b/src/robot.rs index c4fe497..c3b0fae 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -62,7 +62,7 @@ const LEFT_ARROW_STRING: &str = "\x7F"; const RIGHT_ARROW_STRING: &str = "\x7E"; const DEGREES_STRING: &str = "\x03"; -const DISPLAY_RESET_DELAY_MS: u32 = 5000; +const DISPLAY_RESET_DELAY_MS: u64 = 5000; const CONFIG_DIRECTORY: &str = "config"; const CONFIG_FILE: &str = "config.ini"; @@ -86,8 +86,8 @@ static RIGHT_WHEEL_COUNTER: Mutex> = Mutex::new(Cell::new(0)); // Constants for the robot // -const BUTTON_DEBOUNCE_TIME_MS: u32 = 10; -const CONTROLLER_SAMPLE_PERIOD_MS: u32 = 25; +const BUTTON_DEBOUNCE_TIME_MS: u64 = 10; +const CONTROLLER_SAMPLE_PERIOD_MS: u64 = 25; pub struct Robot< 'a, @@ -113,7 +113,7 @@ pub struct Robot< HeadingCalculator>, lcd: AdafruitLCDBackpack, Timer>, pub sd_card: FileStorage, - reset_display_start_millis: u32, + reset_display_start_millis: u64, log_index: u32, pub logger: Logger, idle_message_line2: Option, @@ -313,6 +313,7 @@ where if self.reset_display_start_millis != 0 && millis() - self.reset_display_start_millis > DISPLAY_RESET_DELAY_MS { + self.heading_calculator.update(); debug!("Resetting LCD to idle message"); let idle_message = self.config.idle_message.clone(); if let Err(error) = @@ -437,7 +438,7 @@ where .ok(); let mut traveled_ticks: u32 = 0; - let mut last_update_millis = 0; + let mut last_update_millis: u64 = 0; let mut left_wheel_ticks = 0; let mut right_wheel_ticks = 0; @@ -614,6 +615,7 @@ where error!("Error writing to LCD: {}", error.to_string().as_str()); } self.heading_calculator.reset(); + self.handle_loop(); let mut current_angle = self.heading_calculator.heading(); let mut last_adjust_angle = current_angle; diff --git a/src/robot/debouncer.rs b/src/robot/debouncer.rs index 8f7b15e..b6eec5a 100644 --- a/src/robot/debouncer.rs +++ b/src/robot/debouncer.rs @@ -5,14 +5,14 @@ use embedded_hal::digital::InputPin; /// A debounced button. The button is considered pressed when the pin is equal to the level indicated by `ACTIVE`. /// `ACTIVE` being true indicates the button is active HIGH, and false indicates active LOW. A debounce /// period of `DEBOUNCE` milliseconds is used to prevent bouncing. -pub struct DebouncedButton { +pub struct DebouncedButton { pin: PIN, state: bool, press_consumed: bool, - last_change: u32, + last_change: u64, } -impl +impl DebouncedButton { /// Create a new debounced button. diff --git a/src/robot/file_storage.rs b/src/robot/file_storage.rs index bf18f77..a3b37e5 100644 --- a/src/robot/file_storage.rs +++ b/src/robot/file_storage.rs @@ -218,25 +218,22 @@ where { fn drop(&mut self) { info!("FileStorage dropped"); - match &mut self.volume_mgr { - Some(volume_mgr) => { - if let Err(e) = volume_mgr - .as_ref() - .borrow_mut() - .close_dir(self.root_dir.unwrap()) - { - error!("Error closing root dir: {:?}", e); - return; - } - if let Err(e) = volume_mgr - .as_ref() - .borrow_mut() - .close_volume(self.volume.unwrap()) - { - error!("Error closing volume: {:?}", e); - } + if let Some(volume_mgr) = &mut self.volume_mgr { + if let Err(e) = volume_mgr + .as_ref() + .borrow_mut() + .close_dir(self.root_dir.unwrap()) + { + error!("Error closing root dir: {:?}", e); + return; + } + if let Err(e) = volume_mgr + .as_ref() + .borrow_mut() + .close_volume(self.volume.unwrap()) + { + error!("Error closing volume: {:?}", e); } - None => {} } } } diff --git a/src/robot/telemetry/straight_movement.rs b/src/robot/telemetry/straight_movement.rs index c57f05b..31928bf 100644 --- a/src/robot/telemetry/straight_movement.rs +++ b/src/robot/telemetry/straight_movement.rs @@ -1,6 +1,6 @@ #[derive(Copy, Clone, Default)] pub struct StraightTelemetryRow { - time: u32, + time: u64, left_wheel_ticks: u32, right_wheel_ticks: u32, left_motor_power: u8, @@ -25,7 +25,7 @@ impl StraightTelemetryRow { } pub fn new( - time: u32, + time: u64, left_wheel_ticks: u32, right_wheel_ticks: u32, left_motor_power: u8, diff --git a/src/system/millis.rs b/src/system/millis.rs index 0fe122c..d50e0ab 100644 --- a/src/system/millis.rs +++ b/src/system/millis.rs @@ -8,13 +8,14 @@ use rp_pico::hal::{ }; static MILLIS_ALARM: Mutex>> = Mutex::new(RefCell::new(None)); -static MILLIS_COUNT: Mutex> = Mutex::new(Cell::new(0)); +static MILLIS_TENTH_COUNT: Mutex> = Mutex::new(Cell::new(0)); +const MILLIS_ALARM_FREQUENCY_HZ: u32 = 10000; /// Initialize the millis timer. This will enable the `TIMER_IRQ_0` interrupt and consumes the `Alarm0` alarm. pub fn init_millis(timer: &mut Timer) -> Result<(), ScheduleAlarmError> { cortex_m::interrupt::free(|cs| { let mut alarm0 = timer.alarm_0().unwrap(); - alarm0.schedule(MicrosDurationU32::Hz(1000))?; + alarm0.schedule(MicrosDurationU32::Hz(MILLIS_ALARM_FREQUENCY_HZ))?; alarm0.enable_interrupt(); MILLIS_ALARM.borrow(cs).replace(Some(alarm0)); @@ -27,8 +28,13 @@ pub fn init_millis(timer: &mut Timer) -> Result<(), ScheduleAlarmError> { } /// Get the current millis count. -pub fn millis() -> u32 { - cortex_m::interrupt::free(|cs| MILLIS_COUNT.borrow(cs).get()) +pub fn millis() -> u64 { + let millis_tenth = cortex_m::interrupt::free(|cs| MILLIS_TENTH_COUNT.borrow(cs).get()); + millis_tenth / 10 +} + +pub fn millis_tenths() -> u64 { + cortex_m::interrupt::free(|cs| MILLIS_TENTH_COUNT.borrow(cs).get()) } #[interrupt] @@ -36,10 +42,12 @@ fn TIMER_IRQ_0() { cortex_m::interrupt::free(|cs| { if let Some(alarm) = MILLIS_ALARM.borrow(cs).borrow_mut().as_mut() { alarm.clear_interrupt(); - alarm.schedule(MicrosDurationU32::Hz(1000)).ok(); - MILLIS_COUNT + alarm + .schedule(MicrosDurationU32::Hz(MILLIS_ALARM_FREQUENCY_HZ)) + .ok(); + MILLIS_TENTH_COUNT .borrow(cs) - .set(MILLIS_COUNT.borrow(cs).get() + 1); + .set(MILLIS_TENTH_COUNT.borrow(cs).get() + 1); }; }); }