Support for SD Card

Cargo.toml

@@ -13,20 +13,19 @@ bench = false
 cortex-m = "0.7"
 cortex-m-rt = "0.7"
 embedded-hal = { version = "0.2", features = ["unproven"] }
-
+embedded-sdmmc = { version = "0.6", default-features = false, features = [
+    "defmt-log",
+] }
+shared-bus = { version = "0.3", features = ["cortex-m"] }
 defmt = "0.3"
 defmt-rtt = "0.4"
 panic-probe = { version = "0.3", features = ["print-defmt"] }
 micromath = { version = "2.1", features = ["vector"] }
 mpu6050 = { git = "https://github.com/michaelkamprath/mpu6050.git", branch = "micromath", features = [
     "defmt",
-    "shared_i2c",
 ] }
 embedded-alloc = "0.5"
-adafruit-lcd-backpack = { git = "https://github.com/michaelkamprath/adafruit-lcd-backpack-rust.git", features = [
-    "shared_i2c",
-    "defmt",
-] }
+adafruit-lcd-backpack = { version = "0.1", features = ["defmt"] }
 
 # We're using a Pico by default on this template
 rp-pico = "0.8"
@@ -96,3 +95,6 @@ opt-level = 3
 
 # [patch."https://github.com/michaelkamprath/mpu6050.git"]
 # mpu6050 = { path = "../mpu6050" }
+
+# [patch."https://github.com/michaelkamprath/adafruit-lcd-backpack-rust.git"]
+# adafruit-lcd-backpack = { path = "../adafruit-lcd-backpack-rust" }

src/driver.rs

@@ -1,18 +1,20 @@
-#![allow(non_camel_case_types, dead_code)]
-use core::{cell::RefCell, fmt::Write};
+#![allow(non_camel_case_types, dead_code, clippy::type_complexity)]
+use core::fmt::Write;
 
 use crate::{model::point::Point, robot::Robot, system::millis::millis};
-use alloc::rc::Rc;
 use defmt::{debug, warn};
 use embedded_hal::{
-    blocking::delay::{DelayMs, DelayUs},
-    blocking::i2c::{Write as I2cWrite, WriteRead},
+    blocking::{
+        delay::{DelayMs, DelayUs},
+        i2c::{Write as I2cWrite, WriteRead},
+    },
     digital::v2::{InputPin, OutputPin},
     PwmPin,
 };
 use micromath::F32Ext;
 
 pub struct Driver<
+    'a,
     INA1: OutputPin,
     INA2: OutputPin,
     INB1: OutputPin,
@@ -22,15 +24,26 @@ pub struct Driver<
     BUTT1: InputPin,
     BUTT2: InputPin,
     TWI,
+    TWI_ERR,
+    SPI,
+    CS: OutputPin,
     DELAY,
     LED1: OutputPin,
-> {
-    robot: Robot<INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, DELAY>,
-    delay: Rc<RefCell<DELAY>>,
+> where
+    TWI: I2cWrite<Error = TWI_ERR> + WriteRead<Error = TWI_ERR>,
+    TWI_ERR: defmt::Format,
+    SPI: embedded_hal::blocking::spi::Transfer<u8> + embedded_hal::blocking::spi::Write<u8>,
+    <SPI as embedded_hal::blocking::spi::Transfer<u8>>::Error: core::fmt::Debug,
+    <SPI as embedded_hal::blocking::spi::Write<u8>>::Error: core::fmt::Debug,
+    DELAY: DelayMs<u16> + DelayUs<u16> + DelayMs<u8> + DelayUs<u8> + Copy,
+{
+    robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY>,
+    delay: DELAY,
     led1: LED1,
 }
 
 impl<
+        'a,
         INA1: OutputPin,
         INA2: OutputPin,
         INB1: OutputPin,
@@ -41,17 +54,37 @@ impl<
         BUTT2: InputPin,
         TWI,
         TWI_ERR,
+        SPI,
+        CS: OutputPin,
         DELAY,
         LED1: OutputPin,
-    > Driver<INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, DELAY, LED1>
+    > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY, LED1>
 where
     TWI: I2cWrite<Error = TWI_ERR> + WriteRead<Error = TWI_ERR>,
     TWI_ERR: defmt::Format,
-    DELAY: DelayMs<u16> + DelayUs<u16> + DelayMs<u8>,
+    SPI: embedded_hal::blocking::spi::Transfer<u8> + embedded_hal::blocking::spi::Write<u8>,
+    <SPI as embedded_hal::blocking::spi::Transfer<u8>>::Error: core::fmt::Debug,
+    <SPI as embedded_hal::blocking::spi::Write<u8>>::Error: core::fmt::Debug,
+    DELAY: DelayMs<u16> + DelayUs<u16> + DelayMs<u8> + DelayUs<u8> + Copy,
 {
     pub fn new(
-        robot: Robot<INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, DELAY>,
-        delay: Rc<RefCell<DELAY>>,
+        robot: Robot<
+            'a,
+            INA1,
+            INA2,
+            INB1,
+            INB2,
+            ENA,
+            ENB,
+            BUTT1,
+            BUTT2,
+            TWI,
+            TWI_ERR,
+            SPI,
+            CS,
+            DELAY,
+        >,
+        delay: DELAY,
         led1_pin: LED1,
     ) -> Self {
         Self {
@@ -67,7 +100,7 @@ where
         let start_millis = millis();
         while millis() - start_millis < ms {
             self.robot.handle_loop();
-            self.delay.borrow_mut().delay_us(500);
+            self.delay.delay_us(500_u16);
         }
     }
 
@@ -78,6 +111,12 @@ where
                 Point::new(0, 0),
                 Point::new(0, 500),
                 Point::new_with_forward(0, 0, false),
+                Point::new(500, 0),
+                Point::new_with_forward(0, 0, false),
+                Point::new(0, -500),
+                Point::new_with_forward(0, 0, false),
+                Point::new(-500, 0),
+                Point::new_with_forward(0, 0, false),
             ]);
             self.led1.set_low().ok();
         }
@@ -99,6 +138,14 @@ where
             warn!("trace_path: point_sequence too short");
             return;
         }
+        writeln!(
+            self.robot.logger,
+            "TRACE_PATH: starting trace path with {} points at {} ms",
+            point_sequence.len(),
+            millis(),
+        )
+        .ok();
+
         let mut cur_point = point_sequence[0];
         // start with the bearing from the first point to the second point
         let mut cur_bearing: f32 = point_sequence[0].absolute_bearing(&point_sequence[1]);
@@ -108,7 +155,12 @@ where
             let bearing = cur_point.absolute_bearing(next_point);
             let bearing_diff = bearing - cur_bearing;
             debug!("driving to next way point\n  cur_point: {:?}, next_point: {:?}\n  distance: {}, bearing {}, forward: {}", cur_point, next_point, distance, bearing, next_point.forward());
-
+            writeln!(
+                self.robot.logger,
+                "MOVE: from way point {:?} to way point {:?}",
+                cur_point, next_point
+            )
+            .ok();
             // turn to the correct bearing
             if bearing_diff.abs() > self.robot.min_turn_angle() {
                 self.robot.turn(bearing_diff as i32);

src/main.rs

@@ -5,15 +5,20 @@ mod model;
 mod robot;
 mod system;
 
-use alloc::rc::Rc;
 use bsp::{
     entry,
     hal::{fugit::HertzU32, gpio},
 };
-use core::cell::RefCell;
-use defmt::{info, panic};
+use defmt::{error, info, panic};
 use defmt_rtt as _;
 use panic_probe as _;
+use rp2040_hal::{
+    gpio::{
+        bank0::{Gpio0, Gpio2, Gpio3, Gpio4, Gpio5},
+        FunctionI2c, FunctionSpi, Pin, PullDown,
+    },
+    pac::{I2C0, SPI0},
+};
 
 // Provide an alias for our BSP so we can switch targets quickly.
 // Uncomment the BSP you included in Cargo.toml, the rest of the code does not need to change.
@@ -51,7 +56,7 @@ fn main() -> ! {
 
     info!("Program start");
     let mut pac = pac::Peripherals::take().unwrap();
-    let core = pac::CorePeripherals::take().unwrap();
+    let _core = pac::CorePeripherals::take().unwrap();
     let mut watchdog = Watchdog::new(pac.WATCHDOG);
     let sio = Sio::new(pac.SIO);
 
@@ -69,11 +74,6 @@ fn main() -> ! {
     .ok()
     .unwrap();
 
-    let mut delay_shared = Rc::new(RefCell::new(cortex_m::delay::Delay::new(
-        core.SYST,
-        clocks.system_clock.freq().to_Hz(),
-    )));
-
     let pins = bsp::Pins::new(
         pac.IO_BANK0,
         pac.PADS_BANK0,
@@ -109,27 +109,77 @@ fn main() -> ! {
         &mut pac.RESETS,
         clocks.system_clock.freq(),
     );
+    let i2c_manager: &'static _ = shared_bus::new_cortexm!(rp2040_hal::I2C<I2C0, (Pin<Gpio4, FunctionI2c, PullDown>, Pin<Gpio5, FunctionI2c, PullDown>)> = i2c).unwrap();
+
+    // set up SPI
+    #[allow(clippy::type_complexity)]
+    let spi: rp_pico::hal::Spi<
+        rp_pico::hal::spi::Disabled,
+        SPI0,
+        (
+            rp_pico::hal::gpio::Pin<Gpio3, FunctionSpi, PullDown>,
+            rp_pico::hal::gpio::Pin<Gpio0, FunctionSpi, PullDown>,
+            rp_pico::hal::gpio::Pin<Gpio2, FunctionSpi, PullDown>,
+        ),
+    > = bsp::hal::Spi::new(
+        pac.SPI0,
+        (
+            pins.gpio3.into_function::<gpio::FunctionSpi>(),
+            pins.gpio0.into_function::<gpio::FunctionSpi>(),
+            pins.gpio2.into_function::<gpio::FunctionSpi>(),
+        ),
+    );
 
-    let robot = Robot::new(
+    // Exchange the uninitialised SPI driver for an initialised one
+    let spi = spi.init(
+        &mut pac.RESETS,
+        clocks.peripheral_clock.freq(),
+        HertzU32::from_raw(400_000), // card initialization happens at low baud rate
+        embedded_hal::spi::MODE_0,
+    );
+
+    let sd = crate::robot::file_storage::FileStorage::new(
+        spi,
+        pins.gpio1.into_push_pull_output(),
+        timer,
+    );
+
+    // If SD card is successfully initialized, we can increase the SPI speed
+    match sd.spi(|spi| {
+        spi.set_baudrate(
+            clocks.peripheral_clock.freq(),
+            HertzU32::from_raw(20_000_000),
+        )
+    }) {
+        Some(speed) => {
+            info!("SPI speed increased to {}", speed.raw());
+        }
+        None => {
+            error!("Error increasing SPI speed");
+        }
+    }
+
+    let mut robot = Robot::new(
         pins.gpio10.into_push_pull_output(),
         pins.gpio11.into_push_pull_output(),
         pins.gpio13.into_push_pull_output(),
         pins.gpio12.into_push_pull_output(),
         channel_a,
         channel_b,
-        pins.gpio2.into_pull_up_input(),
-        pins.gpio3.into_pull_up_input(),
-        i2c,
+        pins.gpio14.into_pull_up_input(),
+        pins.gpio15.into_pull_up_input(),
+        i2c_manager,
         pins.gpio21.into_pull_up_input(),
         pins.gpio20.into_pull_up_input(),
-        &mut delay_shared,
+        sd,
+        &mut timer,
     );
 
-    let mut driver = Driver::new(
-        robot,
-        delay_shared.clone(),
-        pins.led.into_push_pull_output(),
-    );
+    if let Err(e) = robot.init() {
+        panic!("Error initializing SD card: {:?}", e);
+    }
+
+    let mut driver = Driver::new(robot, timer, pins.led.into_push_pull_output());
 
     info!("robot controller and driver created");
 

src/model/heading.rs

@@ -1,8 +1,7 @@
 #![allow(non_camel_case_types)]
 
 use crate::system::millis::millis;
-use alloc::rc::Rc;
-use core::{borrow::BorrowMut, cell::RefCell, marker::PhantomData};
+use core::marker::PhantomData;
 
 use defmt::{error, info};
 use embedded_hal::blocking::{
@@ -35,11 +34,9 @@ where
     DELAY: DelayMs<u8>,
     TWI_ERR: defmt::Format,
 {
-    pub fn new(i2c: &Rc<RefCell<TWI>>, delay: &mut Rc<RefCell<DELAY>>) -> Self {
-        let mut gyro = Mpu6050::new(i2c.clone());
-        let delay_borrowed = delay.borrow_mut();
-        let delay_ref = &mut *(**delay_borrowed).borrow_mut();
-        match gyro.init(delay_ref) {
+    pub fn new(i2c: TWI, delay: &mut DELAY) -> Self {
+        let mut gyro = Mpu6050::new(i2c);
+        match gyro.init(delay) {
             Ok(_) => {
                 info!("Gyro initialized");
             }

src/model/point.rs

@@ -45,17 +45,18 @@ impl Point {
         let x_diff = -(other.x - self.x);
         let y_diff = other.y - self.y;
         let bearing = (x_diff as f32).atan2(y_diff as f32).to_degrees();
-        if other.forward {
+        let raw_bearing = if other.forward {
             bearing
         } else {
-            let reverse = bearing + 180.0;
-            if reverse > 180.0 {
-                reverse - 360.0
-            } else if reverse < -180.0 {
-                reverse + 360.0
-            } else {
-                reverse
-            }
+            bearing + 180.0
+        };
+
+        if raw_bearing > 180.0 {
+            raw_bearing - 360.0
+        } else if raw_bearing < -180.0 {
+            raw_bearing + 360.0
+        } else {
+            raw_bearing
         }
     }
 }
@@ -65,3 +66,9 @@ impl Format for Point {
         defmt::write!(f, "({}, {})", self.x, self.y);
     }
 }
+
+impl core::fmt::Display for Point {
+    fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result {
+        write!(f, "({}, {})", self.x, self.y)
+    }
+}