diff --git a/src/driver.rs b/src/driver.rs index a8db4ca..ac32f5b 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -31,13 +31,12 @@ pub struct Driver< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0: I2c, TWI1: I2c, SPI_DEV: SpiDevice, DELAY: DelayNs + Clone, LED1: OutputPin, > { - robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY>, + robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY>, delay: DELAY, led1: LED1, selected_path: Option, @@ -53,29 +52,14 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0: I2c, TWI1: I2c, SPI_DEV: SpiDevice, DELAY: DelayNs + Clone, LED1: OutputPin, - > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY, LED1> + > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY, LED1> { pub fn new( - robot: Robot< - 'a, - INA1, - INA2, - INB1, - INB2, - ENA, - ENB, - BUTT1, - BUTT2, - TWI0, - TWI1, - SPI_DEV, - DELAY, - >, + robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY>, delay: DELAY, led1_pin: LED1, ) -> Self { @@ -234,9 +218,8 @@ impl< // - button one will select the current function and execute it // - if no button is pressed for the idle wait time, return to idle state - const FUNCTIONS: [&str; 6] = [ + const FUNCTIONS: [&str; 5] = [ "Select Path", - "Calibrate Gyro", "Heading Display", "Drive Straight", "Turn Left", @@ -282,20 +265,10 @@ impl< self.robot.start_display_reset_timer(); } 1 => { - write!( - self.robot.clear_lcd().set_lcd_cursor(0, 0), - "Calibrating gyro", - ) - .ok(); - self.robot.calibrate_gyro(&mut self.delay); - write!(self.robot.set_lcd_cursor(0, 1), " Done ",).ok(); - self.robot.start_display_reset_timer(); - } - 2 => { self.robot.display_heading().ok(); self.robot.set_display_to_idle(); } - 3 => { + 2 => { write!( self.robot.clear_lcd().set_lcd_cursor(0, 0), "Driving straight", @@ -305,7 +278,7 @@ impl< self.robot.straight(1500, true); self.robot.start_display_reset_timer(); } - 4 => { + 3 => { write!( self.robot.clear_lcd().set_lcd_cursor(0, 0), "Turning left \x7F", @@ -315,7 +288,7 @@ impl< self.robot.turn(90); self.robot.start_display_reset_timer(); } - 5 => { + 4 => { write!( self.robot.clear_lcd().set_lcd_cursor(0, 0), "Turning right \x7E", diff --git a/src/main.rs b/src/main.rs index 554cb33..8ce8b11 100644 --- a/src/main.rs +++ b/src/main.rs @@ -6,7 +6,7 @@ mod robot; mod system; use core::cell::RefCell; -use defmt::{error, info, panic}; +use defmt::{info, panic}; use defmt_rtt as _; use panic_probe as _; @@ -24,7 +24,7 @@ use bsp::{ }, }; use driver::Driver; -use rp_pico as bsp; +use rp_pico::{self as bsp, hal::multicore::Multicore}; use system::millis::init_millis; extern crate alloc; @@ -39,14 +39,17 @@ fn main() -> ! { use core::mem::MaybeUninit; const HEAP_SIZE: usize = 4048; static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; - unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + #[allow(static_mut_refs)] + 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(); let mut watchdog = Watchdog::new(pac.WATCHDOG); - let sio = Sio::new(pac.SIO); + let mut sio = Sio::new(pac.SIO); // External high-speed crystal on the pico board is 12Mhz let external_xtal_freq_hz = 12_000_000u32; @@ -61,6 +64,7 @@ fn main() -> ! { ) .ok() .unwrap(); + let sys_freq = clocks.system_clock.freq(); let pins = bsp::Pins::new( pac.IO_BANK0, @@ -94,16 +98,16 @@ fn main() -> ! { let sda1_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio6.reconfigure(); let scl1_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio7.reconfigure(); // set up I2C - let i2c0 = bsp::hal::I2C::new_controller( - pac.I2C0, - sda0_pin, - scl0_pin, - HertzU32::from_raw(400_000), - &mut pac.RESETS, - clocks.system_clock.freq(), - ); - let i2c0_ref_cell = RefCell::new(i2c0); - let i2c0_mutex = critical_section::Mutex::new(i2c0_ref_cell); + // let i2c0 = bsp::hal::I2C::new_controller( + // pac.I2C0, + // sda0_pin, + // scl0_pin, + // HertzU32::from_raw(400_000), + // &mut pac.RESETS, + // clocks.system_clock.freq(), + // ); + // let i2c0_ref_cell = RefCell::new(i2c0); + // let i2c0_mutex = critical_section::Mutex::new(i2c0_ref_cell); let i2c1 = bsp::hal::I2C::new_controller( pac.I2C1, @@ -130,30 +134,30 @@ fn main() -> ! { HertzU32::from_raw(400_000), // card initialization happens at low baud rate embedded_hal::spi::MODE_0, ); + let spi_mutex = critical_section::Mutex::new(RefCell::new(spi)); - let sd = crate::robot::file_storage::FileStorage::new( - crate::robot::file_storage::sd_card_spi_device::SDCardSPIDevice::new( - spi, - pins.gpio1.into_push_pull_output(), - timer, - ), + let sd_card_device = embedded_hal_bus::spi::CriticalSectionDevice::new( + &spi_mutex, + pins.gpio1.into_push_pull_output(), timer, ); + let sd = crate::robot::file_storage::FileStorage::new(sd_card_device, timer); // If SD card is successfully initialized, we can increase the SPI speed - match sd.spi(|spi| { - spi.bus.set_baudrate( - clocks.peripheral_clock.freq(), - HertzU32::from_raw(16_000_000), - ) - }) { - Some(speed) => { - info!("SPI speed increased to {}", speed.raw()); - } - None => { - error!("Error increasing SPI speed"); - } - } + info!( + "SPI speed increased to {}", + critical_section::with(|cs| { + spi_mutex.borrow_ref_mut(cs).set_baudrate( + clocks.peripheral_clock.freq(), + HertzU32::from_raw(16_000_000), + ) + }) + .raw() + ); + // get core1 ready + let mut mc = Multicore::new(&mut pac.PSM, &mut pac.PPB, &mut sio.fifo); + let cores = mc.cores(); + let core1 = &mut cores[1]; let mut robot = Robot::new( pins.gpio10.into_push_pull_output(), @@ -164,16 +168,18 @@ fn main() -> ! { channel_b, pins.gpio14.into_pull_up_input(), pins.gpio15.into_pull_up_input(), - &i2c0_mutex, &i2c1_mutex, pins.gpio21.into_pull_up_input(), pins.gpio20.into_pull_up_input(), sd, &mut timer, + core1, + sys_freq, ); - if let Err(e) = robot.init() { - panic!("Error initializing SD card: {:?}", e); + let led_pin = pins.gpio16.into_push_pull_output(); + if let Err(e) = robot.init(sda0_pin, scl0_pin, led_pin) { + panic!("Error initializing Robot: {:?}", e); } let mut driver = Driver::new(robot, timer, pins.led.into_push_pull_output()); diff --git a/src/model/heading.rs b/src/model/heading.rs deleted file mode 100644 index 38a83f8..0000000 --- a/src/model/heading.rs +++ /dev/null @@ -1,147 +0,0 @@ -#![allow(non_camel_case_types)] - -use crate::system::millis::millis_tenths; - -use defmt::{error, info}; -use embedded_hal::{delay::DelayNs, i2c::I2c}; -use micromath::vector::Vector3d; -use mpu6050::Mpu6050; - -// My MCU6050 calibration values -// .. XAccel YAccel ZAccel XGyro YGyro ZGyro -// [-2281,-2280] --> [-3,14] [1725,1726] --> [-15,4] [1581,1581] --> [16381,16389] [97,98] --> [-2,1] [43,44] --> [-1,1] [20,20] --> [0,1] -// [-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_TENTHS: u64 = 20; - -pub struct HeadingCalculator { - heading: f64, - gyro: Mpu6050, - last_update_rate: f64, - last_update_millis_tenths: u64, - inited: bool, -} - -#[allow(dead_code, non_camel_case_types)] -impl HeadingCalculator -where - TWI: I2c, -{ - pub fn new(i2c: TWI, timer: &mut DELAY) -> Self - where - DELAY: DelayNs, - { - let mut gyro = Mpu6050::new(i2c); - let mut inited = false; - match gyro.init(timer) { - Ok(_) => { - info!("Gyro initialized"); - inited = true; - } - Err(_e) => { - error!("Error initializing gyro"); - } - }; - if inited { - if let Err(_error) = gyro.set_gyro_range(mpu6050::device::GyroRange::D250) { - error!("Error setting gyro range"); - } - - let cur_offsets = match gyro.get_gyro_offsets() { - Ok(offsets) => offsets, - Err(_error) => { - error!("Error getting gyro offsets"); - Vector3d::::default() - } - }; - - info!( - "Got gyro offsets: x = {}, y = {}, z = {}", - cur_offsets.x, cur_offsets.y, cur_offsets.z - ); - } - - Self { - heading: 0.0, - gyro, - last_update_rate: 0.0, - last_update_millis_tenths: millis_tenths(), - inited, - } - } - - pub fn is_inited(&self) -> bool { - self.inited - } - - pub fn calibrate(&mut self, delay: &mut DELAY, callback: F) { - if !self.is_inited() { - error!("Gyro not initialized"); - return; - } - if let Err(_e) = self.gyro.calibrate_gyro(delay, callback) { - error!("Error calibrating gyro"); - } - } - - pub fn set_gyro_offsets(&mut self, x_offset: i16, y_offset: i16, z_offset: i16) { - if !self.is_inited() { - error!("Cannot set gyro offsets before gyro is initialized"); - return; - } - if self - .gyro - .set_gyro_offsets(x_offset, y_offset, z_offset) - .is_err() - { - error!("Error setting gyro offsets"); - } - info!( - "Set gyro offsets: x = {}, y = {}, z = {}", - x_offset, y_offset, z_offset - ); - } - - pub fn reset(&mut self) { - self.heading = 0.0; - self.last_update_rate = 0.0; - self.last_update_millis_tenths = millis_tenths(); - } - - pub fn update(&mut self) -> f64 { - if !self.is_inited() { - return 0.0; - } - 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_tenths = now; - // the heding is about the sensor's Z-axis - let delta_degs = (self.last_update_rate + gyro.z as f64) - * (delta_time as f64 / 10000.0) - / 2.0; - self.heading += delta_degs; - self.last_update_rate = gyro.z as f64; - - if self.heading > 180.0 { - self.heading -= 360.0; - } else if self.heading < -180.0 { - self.heading += 360.0; - } - } - Err(_error) => { - error!("Error reading gyro"); - } - } - } - - self.heading - } - - pub fn heading(&mut self) -> f64 { - self.update() - } -} diff --git a/src/model/heading_core1.rs b/src/model/heading_core1.rs new file mode 100644 index 0000000..7245913 --- /dev/null +++ b/src/model/heading_core1.rs @@ -0,0 +1,210 @@ +use bsp::hal::{ + fugit::{HertzU32, Rate}, + gpio::{ + bank0::{Gpio4, Gpio5}, + FunctionI2C, Pin, PullUp, + }, + multicore::Stack, + pac, Timer, +}; +use core::cell::{Cell, RefCell}; +use critical_section::Mutex; +use defmt::error; +use embedded_hal::digital::OutputPin; +use micromath::vector::Vector3d; +use mpu6050::Mpu6050; +use rp_pico as bsp; + +const HEADING_UPDATE_MICROS: u32 = 5 * 1000; // 5ms + +/// Stack for core 1 +static mut CORE1_STACK: Stack<4096> = Stack::new(); + +static HEADING_VALUE: Mutex> = Mutex::new(Cell::new(HeadingData { + heading: 0.0, + last_update_rate: 0.0, + last_update_ticks: 0, +})); + +static GYRO_OFFSETS: Mutex>> = + Mutex::new(Cell::new(Vector3d:: { x: 0, y: 0, z: 0 })); + +#[derive(Debug, Clone, Copy)] +struct HeadingData { + heading: f64, + last_update_rate: f64, + last_update_ticks: u64, +} + +/// A core 0 object that manages the heading calculation on core 1. +pub struct HeadingManager<'a> { + core1: &'a mut rp_pico::hal::multicore::Core<'a>, + sys_freq: Rate, + timer: Timer, +} + +impl<'a> HeadingManager<'a> { + pub fn new( + core1: &'a mut rp_pico::hal::multicore::Core<'a>, + sys_freq: Rate, + timer: &mut Timer, + ) -> Self { + Self { + core1, + sys_freq, + timer: *timer, + } + } + + /// Start the heading calculation task on core 1, initializing I2C bus 0 and the gyro sensor. + pub fn start_heading_calculation( + &mut self, + timer: Timer, + i2c0_sda_pin: Pin, + i2c0_scl_pin: Pin, + mut led_pin: LEDPIN, + gyro_offsets: Vector3d, + ) { + critical_section::with(|cs| { + GYRO_OFFSETS.borrow(cs).set(gyro_offsets); + }); + + // spawn the heading calculation task on core1 + let sys_freq = self.sys_freq; // value that can be moved into the closure + let mut timer = timer; + #[allow(static_mut_refs)] + let _res = self.core1.spawn(unsafe { &mut CORE1_STACK.mem }, move || { + // LED is used to indicate that the gyro is good to go. make sure it is off to start + if let Err(_e) = led_pin.set_low() { + error!("Error setting LED pin low"); + }; + + // do the initialization in a critical section + // set up environment + let mut pac = unsafe { pac::Peripherals::steal() }; + let core = unsafe { pac::CorePeripherals::steal() }; + + let mut delay = cortex_m::delay::Delay::new(core.SYST, sys_freq.to_Hz()); + // set up the I2C controller + let i2c0 = bsp::hal::I2C::new_controller( + pac.I2C0, + i2c0_sda_pin, + i2c0_scl_pin, + HertzU32::from_raw(400_000), + &mut pac.RESETS, + sys_freq, + ); + let i2c0_ref_cell = RefCell::new(i2c0); + let i2c0_mutex = critical_section::Mutex::new(i2c0_ref_cell); + + // set up the gyro sensor ] + let mut gyro = Mpu6050::new(embedded_hal_bus::i2c::CriticalSectionDevice::new( + &i2c0_mutex, + )); + if let Err(_e) = gyro.init(&mut timer) { + panic!("Error initializing gyro"); + } + + // sett the offsets + let gyro_offsets = critical_section::with(|cs| GYRO_OFFSETS.borrow(cs).get()); + // if the offsets are all zero, then we calibrate. otherwise, we set the offsets + if gyro_offsets.x == 0 && gyro_offsets.y == 0 && gyro_offsets.z == 0 { + if let Err(_e) = gyro.calibrate_gyro(&mut timer, |count| { + defmt::info!("Calibrating gyro: {}", count); + }) { + defmt::error!("Error calibrating gyro"); + } + } else { + defmt::debug!( + "Setting gyro offsets: {{x: {}, y: {}, z: {} }}", + gyro_offsets.x, + gyro_offsets.y, + gyro_offsets.z + ); + if let Err(_e) = + gyro.set_gyro_offsets(gyro_offsets.x, gyro_offsets.y, gyro_offsets.z) + { + defmt::error!("Error setting gyro offsets"); + } + } + defmt::info!("Gyro initialized : {}", gyro); + if let Err(_e) = led_pin.set_high() { + error!("Error setting LED pin high"); + }; + + critical_section::with(|cs| { + HEADING_VALUE.borrow(cs).set(HeadingData { + heading: 0.0, + last_update_rate: 0.0, + last_update_ticks: timer.get_counter().ticks(), + }); + }); + + // start the heading calculation task + loop { + delay.delay_us(HEADING_UPDATE_MICROS / 10); + HeadingManager::update_heading(&mut gyro, &mut timer); + } + }); + } + /// runs on core 1 + fn update_heading(gyro: &mut Mpu6050, timer: &mut Timer) + where + I2C: embedded_hal::i2c::I2c, + { + let last_data = critical_section::with(|cs| HEADING_VALUE.borrow(cs).get()); + if timer.get_counter().ticks() - last_data.last_update_ticks < HEADING_UPDATE_MICROS as u64 + { + // too soon to update the heading + return; + } + + let gyro_reading = match gyro.get_gyro_deg() { + Ok(reading) => reading, + Err(_e) => { + defmt::error!("Error getting gyro readings vis I2C"); + return; + } + }; + let current_micros = timer.get_counter().ticks(); + + let delta_micros = current_micros - last_data.last_update_ticks; + let delta_time_s = delta_micros as f64 / 1_000_000.0; + + let delta_heading = + (last_data.last_update_rate + gyro_reading.z as f64) * delta_time_s / 2.0; + let mut new_heading = last_data.heading + delta_heading; + if new_heading > 180.0 { + new_heading -= 360.0; + } else if new_heading < -180.0 { + new_heading += 360.0; + } + let new_rate = gyro_reading.z as f64; + critical_section::with(|cs| { + HEADING_VALUE.borrow(cs).set(HeadingData { + heading: new_heading, + last_update_rate: new_rate, + last_update_ticks: current_micros, + }); + }); + } + + /// runs on core 0 + pub fn get_heading(&self) -> f32 { + // DEBUG get status + // send status request to core1 + + critical_section::with(|cs| HEADING_VALUE.borrow(cs).get().heading as f32) + } + + /// reset the heading to 0 + pub fn reset_heading(&self) { + critical_section::with(|cs| { + HEADING_VALUE.borrow(cs).set(HeadingData { + heading: 0.0, + last_update_rate: 0.0, + last_update_ticks: self.timer.get_counter().ticks(), + }); + }); + } +} diff --git a/src/model/mod.rs b/src/model/mod.rs index aff8863..04821fd 100644 --- a/src/model/mod.rs +++ b/src/model/mod.rs @@ -1,3 +1,4 @@ -pub mod heading; +// pub mod heading; +pub mod heading_core1; pub mod pid_controller; pub mod point; diff --git a/src/robot.rs b/src/robot.rs index b3ade60..fffa622 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -6,7 +6,7 @@ mod motor_controller; mod telemetry; use crate::{ - model::{heading::HeadingCalculator, pid_controller::PIDController}, + model::{heading_core1::HeadingManager, pid_controller::PIDController}, robot::{ config::Config, debouncer::DebouncedButton, @@ -36,9 +36,18 @@ use embedded_hal::{ }; use embedded_hal_bus::i2c::CriticalSectionDevice; use i2c_character_display::{AdafruitLCDBackpack, LcdDisplayType}; -use micromath::F32Ext; -use rp_pico::hal::{gpio, Timer}; +use micromath::{vector::Vector3d, F32Ext}; use rp_pico::hal::{ + fugit::Rate, + gpio::{ + self, + bank0::{Gpio4, Gpio5}, + FunctionI2C, PullUp, + }, + Timer, +}; +use rp_pico::hal::{ + gpio::Pin, gpio::{ bank0::{Gpio20, Gpio21}, FunctionSio, Interrupt, SioInput, @@ -99,20 +108,16 @@ pub struct Robot< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: embedded_hal::spi::SpiDevice, DELAY, > where - TWI0: embedded_hal::i2c::I2c, TWI1: embedded_hal::i2c::I2c, DELAY: DelayNs, { motors: MotorController, button1: DebouncedButton, button2: DebouncedButton, - pub heading_calculator: - HeadingCalculator>, lcd: AdafruitLCDBackpack, Timer>, pub sd_card: FileStorage, reset_display_start_millis: u64, @@ -121,6 +126,7 @@ pub struct Robot< idle_message_line2: Option, config: Config, delay: Timer, + heading_core1: HeadingManager<'a>, } #[allow(dead_code, non_camel_case_types)] @@ -134,13 +140,11 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: embedded_hal::spi::SpiDevice, DELAY, - > Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY> + > Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where - TWI0: embedded_hal::i2c::I2c, TWI1: embedded_hal::i2c::I2c, DELAY: DelayNs + Clone, { @@ -154,12 +158,13 @@ where duty_b: ENB, button1_pin: BUTT1, button2_pin: BUTT2, - i2c0_refcell: &'a critical_section::Mutex>, i2c1_refcell: &'a critical_section::Mutex>, left_counter_pin: LeftWheelCounterPin, right_counter_pin: RightWheelCounterPin, mut sd_card: FileStorage, timer: &mut Timer, + core1: &'a mut rp_pico::hal::multicore::Core<'a>, + sys_freq: Rate, ) -> Self { // create the motor controller let motors = MotorController::new(ina1_pin, ina2_pin, inb1_pin, inb2_pin, duty_a, duty_b); @@ -217,11 +222,12 @@ where }; debug!("LCD characters created"); - let mut heading_calculator = HeadingCalculator::new( - embedded_hal_bus::i2c::CriticalSectionDevice::new(i2c0_refcell), - timer, - ); - heading_calculator.reset(); + // let mut heading_calculator = HeadingCalculator::new( + // embedded_hal_bus::i2c::CriticalSectionDevice::new(i2c0_refcell), + // timer, + // core1, + // ); + // heading_calculator.reset(); if let Err(_e) = lcd .home() @@ -240,11 +246,13 @@ where } let logger = Logger::new(Logger::::init_log_file(&mut sd_card)); + + let heading_core1 = HeadingManager::new(core1, sys_freq, timer); + Self { motors, button1: DebouncedButton::::new(button1_pin), button2: DebouncedButton::::new(button2_pin), - heading_calculator, lcd, sd_card, reset_display_start_millis: millis(), // start the display reset timer to clear the "started" message @@ -253,19 +261,25 @@ where idle_message_line2: None, config: Config::new(), delay: *timer, + heading_core1, } } /// Inits the Robot software. This function should be called after the robot is created. - pub fn init(&mut self) -> Result<(), embedded_sdmmc::Error> { + pub fn init( + &mut self, + i2c0_sda_pin: Pin, + i2c0_scl_pin: Pin, + led_pin: LEDPIN, + ) -> Result<(), embedded_sdmmc::Error> { // load configuration from the SD card if self.sd_card.root_dir().is_some() { let root_dir = self.sd_card.root_dir().unwrap(); let config_dir = match self.sd_card.open_directory(root_dir, CONFIG_DIRECTORY) { - Some(d) => d, - None => { - error!("Error opening config directory"); - return Err(embedded_sdmmc::Error::NotFound); + Ok(d) => d, + Err(e) => { + error!("Error opening config directory: {}", e); + return Err(e); } }; if let Ok(mut config_file) = self.sd_card.open_file_in_dir( @@ -283,41 +297,35 @@ where })?; debug!("Config file contents:\n{}", config_str); self.config.set_config_values(config_str); - - // set the gyro offsets from the config file - if self.heading_calculator.is_inited() { - self.heading_calculator.set_gyro_offsets( - self.config.gyro_offset_x, - self.config.gyro_offset_y, - self.config.gyro_offset_z, - ); - } } else { debug!("Config file not found, using defaults"); } } else { warn!("Could not load configuration. Using defaults."); } + self.heading_core1.start_heading_calculation( + self.delay, + i2c0_sda_pin, + i2c0_scl_pin, + led_pin, + Vector3d { + x: self.config.gyro_offset_x, + y: self.config.gyro_offset_y, + z: self.config.gyro_offset_z, + }, + ); writeln!(self.logger, "Robot started\n{}\n", self.config) .map_err(|_e| embedded_sdmmc::SdCardError::WriteError)?; info!("Robot initialized\n{}\n", self.config); Ok(()) } - pub fn calibrate_gyro(&mut self, delay: &mut DELAY) { - self.heading_calculator.calibrate(delay, |step| { - if let Ok(lcd) = self.lcd.set_cursor(0, 1) { - write!(lcd, "iteration: {}", step).ok(); - } - }); - } - /// This function is called in the main loop to allow the robot to handle state updates pub fn handle_loop(&mut self) { if self.reset_display_start_millis != 0 && millis() - self.reset_display_start_millis > DISPLAY_RESET_DELAY_MS { - self.heading_calculator.update(); + // self.heading_calculator.update(); debug!("Resetting LCD to idle message"); let idle_message = self.config.idle_message.clone(); if let Err(error) = @@ -338,7 +346,7 @@ where self.button1.handle_loop(); self.button2.handle_loop(); - self.heading_calculator.update(); + // self.heading_calculator.update(); } pub fn set_idle_message_line2(&mut self, message: Option) { @@ -453,7 +461,7 @@ where ); // we want to go straight, so the setpoint is 0 controller.set_setpoint(0.); - self.heading_calculator.reset(); + // self.heading_calculator.reset(); self.motors.set_duty( self.config.straight_left_power, @@ -467,9 +475,9 @@ where millis(), 0, 0, - 100, - 100, - self.heading_calculator.heading(), + self.config.straight_left_power, + self.config.straight_right_power, + self.heading_core1.get_heading() as f64, 0., ), ) @@ -490,8 +498,9 @@ where if millis() - last_update_millis > CONTROLLER_SAMPLE_PERIOD_MS { last_update_millis = millis(); - let heading = self.heading_calculator.heading(); - let control_signal = controller.update(heading as f32, last_update_millis); + // let heading = self.heading_calculator.heading(); + let heading = self.heading_core1.get_heading(); + let control_signal = controller.update(heading, last_update_millis); let mut cs_indicator: &str = direction_arrow; // positive control signal means turn left, a negative control signal means turn right @@ -518,12 +527,11 @@ where right_wheel_ticks, left_power as u8, right_power as u8, - heading, + heading as f64, control_signal, ), ) .ok(); - self.heading_calculator.update(); let wheel_ticks_per_mm = self.config.wheel_ticks_per_mm; if let Err(error) = core::write!( @@ -535,7 +543,6 @@ where ) { error!("Error writing to LCD: {}", error.to_string().as_str()); } - self.heading_calculator.update(); if let Err(error) = core::write!( self.set_lcd_cursor(0, 1), "{} : cs={:.3}", @@ -572,7 +579,7 @@ where right_wheel_ticks, 0, 0, - self.heading_calculator.heading(), + self.heading_core1.get_heading() as f64, 0., ), ) @@ -618,9 +625,9 @@ where ) { error!("Error writing to LCD: {}", error.to_string().as_str()); } - self.heading_calculator.reset(); + self.heading_core1.reset_heading(); self.handle_loop(); - let mut current_angle = self.heading_calculator.heading() as f32; + let mut current_angle = self.heading_core1.get_heading(); let mut last_adjust_angle = current_angle; // start the motors per right hand rule (postive angle = left turn, negative angle = right turn) @@ -646,7 +653,7 @@ where }; while current_angle.abs() < (turn_degrees.abs() + stop_angle_delta) as f32 { - current_angle = self.heading_calculator.heading() as f32; + current_angle = self.heading_core1.get_heading(); if (current_angle - last_adjust_angle).abs() > 10.0 { last_adjust_angle = current_angle; if let Err(error) = core::write!( @@ -662,11 +669,11 @@ where self.handle_loop(); } - let motors_off_heading = self.heading_calculator.heading(); + let motors_off_heading = self.heading_core1.get_heading(); self.motors.brake(); self.delay.delay_ms(100); self.motors.stop(); - current_angle = self.heading_calculator.heading() as f32; + current_angle = self.heading_core1.get_heading(); if let Err(error) = core::write!( self.clear_lcd().set_lcd_cursor(0, 0), "{} {} / {}\x03", @@ -699,8 +706,9 @@ where &mut self, ) -> Result<(), i2c_character_display::CharacterDisplayError>> { + self.heading_core1.reset_heading(); write!(self.lcd.clear()?.set_cursor(0, 0)?, "Heading:").ok(); - self.heading_calculator.reset(); + // self.heading_calculator.reset(); let mut continue_loop = true; let mut last_update_millis = 0; @@ -709,15 +717,15 @@ where if self.button1_pressed() || self.button2_pressed() { continue_loop = false; } - if millis() - last_update_millis > 500 { - let heading = self.heading_calculator.heading(); - if let Err(error) = core::write!( + if millis() - last_update_millis > 200 { + let heading = self.heading_core1.get_heading(); + if let Err(e) = core::write!( self.lcd.set_cursor(0, 1)?, "{:.2}{: <16}", heading, DEGREES_STRING ) { - error!("Error writing to LCD: {}", error.to_string().as_str()); + error!("Error writing to LCD: {}", e.to_string().as_str()); } last_update_millis = millis(); } @@ -744,7 +752,6 @@ where /// Implements the defmt::Format trait for the Robot struct, allowing the Robot object to be printed with defmt impl< - 'a, INA1: OutputPin, INA2: OutputPin, INB1: OutputPin, @@ -753,14 +760,11 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: SpiDevice, DELAY, - > Format - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY> + > Format for Robot<'_, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where - TWI0: I2c, TWI1: I2c, DELAY: DelayNs + Clone, { @@ -776,7 +780,6 @@ where /// Implement the `core::fmt::Write` trait for the robot, allowing us to use the `core::write!` macro impl< - 'a, INA1: OutputPin, INA2: OutputPin, INB1: OutputPin, @@ -785,14 +788,12 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: SpiDevice, DELAY, > core::fmt::Write - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY> + for Robot<'_, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where - TWI0: I2c, TWI1: I2c, DELAY: DelayNs + Clone, { diff --git a/src/robot/file_storage.rs b/src/robot/file_storage.rs index a3b37e5..b2a4f1c 100644 --- a/src/robot/file_storage.rs +++ b/src/robot/file_storage.rs @@ -1,6 +1,5 @@ #![allow(clippy::type_complexity)] pub mod logger; -pub mod sd_card_spi_device; pub mod sd_file; use alloc::{rc::Rc, string::ToString, vec::Vec}; @@ -130,17 +129,22 @@ where self.root_dir } - pub fn open_directory(&self, parent: RawDirectory, name: &str) -> Option { + pub fn open_directory( + &self, + parent: RawDirectory, + name: &str, + ) -> Result> { match self .volume_mgr - .as_ref()? + .as_ref() + .unwrap() .borrow_mut() .open_dir(parent, name) { - Ok(dir) => Some(dir), + Ok(dir) => Ok(dir), Err(e) => { error!("Error opening directory '{}': {:?}", name, e); - None + Err(e) } } } @@ -195,7 +199,7 @@ where ext: &str, ) -> Result, embedded_sdmmc::Error> { let mut files: Vec = Vec::new(); - let dir = self.open_directory(parent_dir, target_dir).unwrap(); + let dir = self.open_directory(parent_dir, target_dir)?; self.iterate_dir(dir, |entry| { if entry.name.to_string().ends_with(ext) { debug!("Found path file: {:?}", entry.name.to_string().as_str()); diff --git a/src/robot/file_storage/logger.rs b/src/robot/file_storage/logger.rs index 98c148b..3321a6f 100644 --- a/src/robot/file_storage/logger.rs +++ b/src/robot/file_storage/logger.rs @@ -51,9 +51,9 @@ where } }; let log_dir = match sdcard.open_directory(root_dir, LOG_DIR_NAME) { - Some(dir) => dir, - None => { - error!("Error opening log dir"); + Ok(dir) => dir, + Err(e) => { + error!("Error opening log dir: {}", e); return None; } }; @@ -104,9 +104,9 @@ where // write the new log index back to the file. Need to re-open the containing directory first. let log_dir = match sdcard.open_directory(root_dir, LOG_DIR_NAME) { - Some(dir) => dir, - None => { - error!("Error opening log dir"); + Ok(dir) => dir, + Err(e) => { + error!("Error opening log dir: {}", e); return None; } }; @@ -135,9 +135,9 @@ where // now creater the logger let log_filename = format!("log_{}.txt", log_index); let log_dir = match sdcard.open_directory(root_dir, LOG_DIR_NAME) { - Some(dir) => dir, - None => { - error!("Error opening log dir"); + Ok(dir) => dir, + Err(e) => { + error!("Error opening log dir: {}", e); return None; } }; diff --git a/src/robot/file_storage/sd_card_spi_device.rs b/src/robot/file_storage/sd_card_spi_device.rs deleted file mode 100644 index 9283c20..0000000 --- a/src/robot/file_storage/sd_card_spi_device.rs +++ /dev/null @@ -1,134 +0,0 @@ -use defmt::{error, trace}; -use embedded_hal::{ - delay::DelayNs, - digital::OutputPin, - spi::{ErrorType, Operation, SpiBus, SpiDevice}, -}; - -#[derive(Debug)] -pub enum SDCardSPIDeviceError { - Spi, - Delay, - ChipSelect, // Add other error variants as needed -} - -impl embedded_hal::spi::Error for SDCardSPIDeviceError { - fn kind(&self) -> embedded_hal::spi::ErrorKind { - match self { - SDCardSPIDeviceError::Spi => embedded_hal::spi::ErrorKind::Other, - SDCardSPIDeviceError::Delay => embedded_hal::spi::ErrorKind::Other, - SDCardSPIDeviceError::ChipSelect => embedded_hal::spi::ErrorKind::ChipSelectFault, - // Map other error variants as needed - } - } -} - -impl core::fmt::Display for SDCardSPIDeviceError { - fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result { - match self { - SDCardSPIDeviceError::Spi => write!(f, "SD Card SPI bus error"), - SDCardSPIDeviceError::Delay => write!(f, "SD Card Delay error"), - SDCardSPIDeviceError::ChipSelect => write!(f, "SD Card chip select error"), - // Map other error variants as needed - }?; - Ok(()) - } -} - -pub struct SDCardSPIDevice -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - pub bus: SPI, - cs: CS, - delay: DELAY, -} - -impl ErrorType for SDCardSPIDevice -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - type Error = SDCardSPIDeviceError; -} - -impl SpiDevice for SDCardSPIDevice -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - fn transaction(&mut self, operations: &mut [Operation<'_, u8>]) -> Result<(), Self::Error> { - trace!( - "SDCardSPIDevice::transaction - operations.len = {}", - operations.len() - ); - // Implement the SPI transaction here - if self.cs.set_low().is_err() { - return Err(SDCardSPIDeviceError::ChipSelect); - } - for operation in operations { - if let Err(_e) = match operation { - Operation::Write(data) => { - trace!( - "SDCardSPIDevice::transaction - write data.len = {}", - data.len() - ); - self.bus.write(data) - } - Operation::Transfer(read_buf, write_buf) => { - trace!("SDCardSPIDevice::transaction - transfer read_buf.len = {}, write_buf.len = {}", read_buf.len(), write_buf.len()); - self.bus.transfer(read_buf, write_buf) - } - Operation::Read(data) => { - trace!( - "SDCardSPIDevice::transaction - read data.len = {}", - data.len() - ); - self.bus.read(data) - } - Operation::TransferInPlace(data) => { - trace!( - "SDCardSPIDevice::transaction - transfer_in_place data.len = {}", - data.len() - ); - self.bus.transfer_in_place(data) - } - Operation::DelayNs(time) => { - trace!("SDCardSPIDevice::transaction - delay_ns time = {}", time); - self.delay.delay_ns(*time); - Ok(()) - } - } { - // if an error occurs, deassert CS pin and return error - self.cs.set_high().ok(); - error!("SDCardSPIDevice::transaction - error"); - return Err(SDCardSPIDeviceError::Spi); - } - } - if self.bus.flush().is_err() { - self.cs.set_high().ok(); - error!("SDCardSPIDevice::transaction - flush error"); - return Err(SDCardSPIDeviceError::Spi); - } - if self.cs.set_high().is_err() { - error!("SDCardSPIDevice::transaction - chip select error"); - return Err(SDCardSPIDeviceError::ChipSelect); - } - Ok(()) - } -} - -impl SDCardSPIDevice -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - pub fn new(bus: SPI, cs: CS, delay: DELAY) -> Self { - Self { bus, cs, delay } - } -} diff --git a/src/robot/file_storage/sd_file.rs b/src/robot/file_storage/sd_file.rs index 57f7196..1fd19c3 100644 --- a/src/robot/file_storage/sd_file.rs +++ b/src/robot/file_storage/sd_file.rs @@ -38,11 +38,13 @@ where pub fn init(&mut self) -> Result<(), embedded_sdmmc::Error> { // first open the file per the passed mode - let file = match self.volume_manager.borrow_mut().open_file_in_dir( - self.directory, - self.filename.as_str(), - self.mode, - ) { + let file = match critical_section::with(|_| { + self.volume_manager.borrow_mut().open_file_in_dir( + self.directory, + self.filename.as_str(), + self.mode, + ) + }) { Ok(f) => { debug!( "Opened file '{}' with mode {}", @@ -57,7 +59,9 @@ where } }; // then close the file - if let Err(e) = self.volume_manager.borrow_mut().close_file(file) { + if let Err(e) = + critical_section::with(|_| self.volume_manager.borrow_mut().close_file(file)) + { error!("Error closing file '{}': {:?}", self.filename.as_str(), e); return Err(e); } diff --git a/src/system/data.rs b/src/system/data.rs index 7ce1a38..b9bff83 100644 --- a/src/system/data.rs +++ b/src/system/data.rs @@ -153,7 +153,7 @@ where } } -impl<'a, T, const M: usize> Format for DataTable<'a, T, M> +impl Format for DataTable<'_, T, M> where T: Copy + Default + Format, { @@ -173,7 +173,7 @@ where } } -impl<'a, T, const M: usize> core::fmt::Display for DataTable<'a, T, M> +impl core::fmt::Display for DataTable<'_, T, M> where T: Copy + Default + Format + core::fmt::Display, { diff --git a/src/system/millis.rs b/src/system/millis.rs index c2a5633..c73fd7a 100644 --- a/src/system/millis.rs +++ b/src/system/millis.rs @@ -28,6 +28,7 @@ pub fn millis() -> u64 { }) } +#[allow(dead_code)] pub fn millis_tenths() -> u64 { cortex_m::interrupt::free(|cs| { MILLIS_TIMER