updated millis count to u64

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

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);
         }

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<TWI> {
     heading: f32,
     gyro: Mpu6050<TWI>,
     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;
 

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;

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<Cell<u32>> = 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<embedded_hal_bus::i2c::CriticalSectionDevice<'a, TWI>>,
     lcd: AdafruitLCDBackpack<CriticalSectionDevice<'a, TWI>, Timer>,
     pub sd_card: FileStorage<SPI_DEV, DELAY>,
-    reset_display_start_millis: u32,
+    reset_display_start_millis: u64,
     log_index: u32,
     pub logger: Logger<SPI_DEV, DELAY, 128>,
     idle_message_line2: Option<String>,
@@ -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;
 

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<PIN: InputPin, const ACTIVE: bool, const DEBOUNCE: u32> {
+pub struct DebouncedButton<PIN: InputPin, const ACTIVE: bool, const DEBOUNCE: u64> {
     pin: PIN,
     state: bool,
     press_consumed: bool,
-    last_change: u32,
+    last_change: u64,
 }
 
-impl<PIN: InputPin, const ACTIVE: bool, const DEBOUNCE: u32>
+impl<PIN: InputPin, const ACTIVE: bool, const DEBOUNCE: u64>
     DebouncedButton<PIN, ACTIVE, DEBOUNCE>
 {
     /// Create a new debounced button.

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 => {}
         }
     }
 }

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,

src/system/millis.rs

@@ -8,13 +8,14 @@ use rp_pico::hal::{
 };
 
 static MILLIS_ALARM: Mutex<RefCell<Option<Alarm0>>> = Mutex::new(RefCell::new(None));
-static MILLIS_COUNT: Mutex<Cell<u32>> = Mutex::new(Cell::new(0));
+static MILLIS_TENTH_COUNT: Mutex<Cell<u64>> = 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,19 +28,26 @@ 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]
 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);
         };
     });
 }