addedsupport for mpu6050 gyro

Cargo.toml

@@ -17,7 +17,11 @@ embedded-hal = { version = "0.2", features = ["unproven"] }
 defmt = "0.3"
 defmt-rtt = "0.4"
 panic-probe = { version = "0.3", features = ["print-defmt"] }
-micromath = "2.1"
+micromath = { version = "2.1", features = ["vector"] }
+mpu6050 = { git = "https://github.com/michaelkamprath/mpu6050.git", branch = "micromath", features = [
+    "defmt",
+] }
+embedded-alloc = "0.5"
 
 # We're using a Pico by default on this template
 rp-pico = "0.8"
@@ -80,3 +84,6 @@ debug-assertions = false
 incremental = false
 lto = 'fat'
 opt-level = 3
+
+[patch."https://github.com/michaelkamprath/mpu6050.git"]
+mpu6050 = { path = "../mpu6050" }

src/main.rs

@@ -19,6 +19,13 @@ use panic_probe as _;
 use rp_pico as bsp;
 // use sparkfun_pro_micro_rp2040 as bsp;
 
+extern crate alloc;
+
+use embedded_alloc::Heap;
+
+#[global_allocator]
+static HEAP: Heap = Heap::empty();
+
 use bsp::hal::{
     clocks::{init_clocks_and_plls, Clock},
     pac,
@@ -32,6 +39,14 @@ use system::millis::{init_millis, millis};
 
 #[entry]
 fn main() -> ! {
+    // Initialize the allocator BEFORE you use it
+    {
+        use core::mem::MaybeUninit;
+        const HEAP_SIZE: usize = 4048;
+        static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
+        unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) }
+    }
+
     info!("Program start");
     let mut pac = pac::Peripherals::take().unwrap();
     let core = pac::CorePeripherals::take().unwrap();
@@ -102,6 +117,7 @@ fn main() -> ! {
         i2c,
         pins.gpio16.into_pull_up_input(),
         pins.gpio17.into_pull_up_input(),
+        &mut delay,
     );
 
     info!("robot controller created");
@@ -115,23 +131,35 @@ fn main() -> ! {
     let mut test_pin = pins.gpio2.into_push_pull_output();
 
     loop {
-        info!("on! millis: {}", millis());
+        info!(
+            "on! millis: {}, heading: {}",
+            millis(),
+            robot.heading_calculator.heading()
+        );
         robot.forward(1.0);
         led_pin.set_high().unwrap();
         test_pin.set_high().unwrap();
         delay.delay_ms(250);
+        robot.handle_loop();
         led_pin.set_low().unwrap();
         delay.delay_ms(100);
+        robot.handle_loop();
         led_pin.set_high().unwrap();
         delay.delay_ms(250);
+        robot.handle_loop();
         led_pin.set_low().unwrap();
         delay.delay_ms(100);
+        robot.handle_loop();
         led_pin.set_high().unwrap();
         delay.delay_ms(250);
+        robot.handle_loop();
         led_pin.set_low().unwrap();
         delay.delay_ms(100);
+        robot.handle_loop();
         led_pin.set_high().unwrap();
         delay.delay_ms(250);
+        robot.handle_loop();
+
         info!("off!");
         robot.stop();
         led_pin.set_low().unwrap();

src/model/heading.rs

@@ -0,0 +1,68 @@
+use crate::system::millis::millis;
+use alloc::rc::Rc;
+use core::cell::RefCell;
+use cortex_m::delay::Delay;
+use defmt::{error, info, trace};
+use embedded_hal::blocking::i2c::{Write, WriteRead};
+use mpu6050::Mpu6050;
+
+pub struct HeadingCalculator<TWI> {
+    heading: f32,
+    gyro: Mpu6050<TWI>,
+    last_update_rate: f32,
+    last_update_millis: u32,
+}
+
+#[allow(dead_code, non_camel_case_types)]
+impl<TWI, TWI_ERR> HeadingCalculator<TWI>
+where
+    TWI: Write<Error = TWI_ERR> + WriteRead<Error = TWI_ERR>,
+{
+    pub fn new(i2c: &Rc<RefCell<TWI>>, delay: &mut Delay) -> Self {
+        let mut gyro = Mpu6050::new(i2c.clone());
+        match gyro.init(delay) {
+            Ok(_) => {
+                info!("Gyro initialized");
+            }
+            Err(e) => {
+                error!("Error initializing gyro: {}", e);
+            }
+        };
+        if let Err(_error) = gyro.set_gyro_range(mpu6050::device::GyroRange::D250) {
+            error!("Error setting gyro range");
+        }
+        Self {
+            heading: 0.0,
+            gyro,
+            last_update_rate: 0.0,
+            last_update_millis: 0,
+        }
+    }
+
+    pub fn reset(&mut self) {
+        self.heading = 0.0;
+        self.last_update_rate = 0.0;
+        self.last_update_millis = millis();
+    }
+
+    pub fn update(&mut self) -> f32 {
+        let now = millis();
+        let delta_time = now - self.last_update_millis;
+        if delta_time > 50 {
+            trace!("update heading");
+            if let Ok(gyro) = self.gyro.get_gyro() {
+                // the heding is about the sensor's Z-axis
+                let delta_rads = gyro.z * delta_time as f32 / 1000.0;
+                self.heading += delta_rads;
+                self.last_update_rate = gyro.z;
+                trace!("delta_rads: {}, heading: {}", delta_rads, self.heading);
+            }
+        }
+
+        self.heading
+    }
+
+    pub fn heading(&mut self) -> f32 {
+        self.update()
+    }
+}

src/model/mod.rs

@@ -1 +1,2 @@
+pub mod heading;
 pub mod pid_controller;

src/robot.rs

@@ -1,9 +1,10 @@
-use crate::l298n::motor_controller::MotorController;
+use crate::{l298n::motor_controller::MotorController, model::heading::HeadingCalculator};
+use alloc::rc::Rc;
 use core::cell::{Cell, RefCell};
-use cortex_m::interrupt::Mutex;
+use cortex_m::{delay::Delay, interrupt::Mutex};
 use defmt::{debug, info, trace};
 use embedded_hal::{
-    blocking::i2c::WriteRead,
+    blocking::i2c::{Write, WriteRead},
     digital::v2::{InputPin, OutputPin},
     PwmPin,
 };
@@ -40,17 +41,18 @@ pub struct Robot<
     ENB: PwmPin<Duty = u16>,
     BUTT1: InputPin,
     BUTT2: InputPin,
-    TWI: WriteRead,
+    TWI,
 > {
     motors: MotorController<INA1, INA2, INB1, INB2, ENA, ENB>,
-    _i2c: TWI,
     button1: BUTT1,
     button2: BUTT2,
     button1_pressed: bool,
     button2_pressed: bool,
+    _i2c: Rc<RefCell<TWI>>,
+    pub heading_calculator: HeadingCalculator<TWI>,
 }
 
-#[allow(dead_code)]
+#[allow(dead_code, non_camel_case_types)]
 impl<
         INA1: OutputPin,
         INA2: OutputPin,
@@ -60,8 +62,11 @@ impl<
         ENB: PwmPin<Duty = u16>,
         BUTT1: InputPin,
         BUTT2: InputPin,
-        TWI: WriteRead,
+        TWI,
+        TWI_ERR,
     > Robot<INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI>
+where
+    TWI: Write<Error = TWI_ERR> + WriteRead<Error = TWI_ERR>,
 {
     #[allow(clippy::too_many_arguments)]
     pub fn new(
@@ -76,6 +81,7 @@ impl<
         i2c: TWI,
         left_counter_pin: LeftWheelCounterPin,
         right_counter_pin: RightWheelCounterPin,
+        delay: &mut Delay,
     ) -> Self {
         // create the motor controller
         let motors = MotorController::new(ina1_pin, ina2_pin, inb1_pin, inb2_pin, ena_pin, enb_pin);
@@ -94,16 +100,21 @@ impl<
         unsafe {
             pac::NVIC::unmask(pac::Interrupt::IO_IRQ_BANK0);
         }
+        let i2c_shared = Rc::new(RefCell::new(i2c));
+
+        let mut heading_calculator = HeadingCalculator::new(&i2c_shared, delay);
+        heading_calculator.reset();
 
         // return the robot
         info!("Robot initialized");
         Self {
             motors,
-            _i2c: i2c,
             button1: button1_pin,
             button2: button2_pin,
             button1_pressed: false,
             button2_pressed: false,
+            _i2c: i2c_shared,
+            heading_calculator,
         }
     }
 
@@ -116,6 +127,8 @@ impl<
         if self.button2.is_high().ok().unwrap() {
             self.button2_pressed = false;
         }
+
+        self.heading_calculator.update();
     }
 
     /// Resets the wheel counters to 0
@@ -191,7 +204,9 @@ impl<
         self
     }
 
-    pub fn straight(&mut self, _distance_mm: u32) -> &mut Self {
+    pub fn straight(&mut self, distance_mm: u32) -> &mut Self {
+        info!("Robot move straight, distance = {}", distance_mm);
+
         self
     }
 

src/system/millis.rs

@@ -10,6 +10,7 @@ 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));
 
+/// 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();
@@ -25,6 +26,7 @@ pub fn init_millis(timer: &mut Timer) -> Result<(), ScheduleAlarmError> {
     Ok(())
 }
 
+/// Get the current millis count.
 pub fn millis() -> u32 {
     cortex_m::interrupt::free(|cs| MILLIS_COUNT.borrow(cs).get())
 }