diff --git a/.cargo/config.toml b/.cargo/config.toml index c909f85..61e6a86 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -27,4 +27,4 @@ rustflags = [ target = "thumbv6m-none-eabi" [env] -DEFMT_LOG = "debug" +DEFMT_LOG = "info" diff --git a/Cargo.toml b/Cargo.toml index 8ec1be9..c10373e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -12,34 +12,35 @@ bench = false [dependencies] 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 = [ +critical-section = "1.1" +embedded-hal = { version = "1.0", features = ["defmt-03"] } +embedded-sdmmc = { version = "0.8", default-features = false, features = [ "defmt-log", ] } -shared-bus = { version = "0.3", features = ["cortex-m"] } +embedded-hal-bus = { version = "0.1.0", features = ["defmt-03"] } 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 = [ +mpu6050 = { git = "https://github.com/michaelkamprath/mpu6050.git", branch = "embedded-hal-upgrade", features = [ "defmt", ] } embedded-alloc = "0.6" -adafruit-lcd-backpack = { version = "0.1", features = ["defmt"] } +i2c-character-display = { version = "0.1", features = ["defmt"] } ini_core = "0.2" # We're using a Pico by default on this template -rp-pico = "0.8" +rp-pico = "0.9" # but you can use any BSP. Uncomment this to use the pro_micro_rp2040 BSP instead # sparkfun-pro-micro-rp2040 = "0.7" # If you're not going to use a Board Support Package you'll need these: -rp2040-hal = { version = "0.9", features = [ - "rt", - "critical-section-impl", - "defmt", -] } +# rp2040-hal = { version = "0.10", features = [ +# "rt", +# "critical-section-impl", +# "defmt", +# ] } # rp2040-boot2 = "0.3" # cargo build/run @@ -94,8 +95,7 @@ incremental = false lto = 'fat' opt-level = 3 -# [patch."https://github.com/michaelkamprath/mpu6050.git"] +[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" } +[patch.crates-io] diff --git a/src/driver.rs b/src/driver.rs index 2dad230..15e3710 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -9,12 +9,11 @@ use alloc::{ }; use defmt::{debug, info, warn}; use embedded_hal::{ - blocking::{ - delay::{DelayMs, DelayUs}, - i2c::{Write as I2cWrite, WriteRead}, - }, - digital::v2::{InputPin, OutputPin}, - PwmPin, + delay::DelayNs, + digital::{InputPin, OutputPin}, + i2c::I2c, + pwm::SetDutyCycle, + spi::SpiDevice, }; use micromath::F32Ext; @@ -28,25 +27,16 @@ pub struct Driver< INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI, - TWI_ERR, - SPI, - CS: OutputPin, - DELAY, + TWI: I2c, + SPI_DEV: SpiDevice, + DELAY: DelayNs + Clone, LED1: OutputPin, -> where - TWI: I2cWrite + WriteRead, - TWI_ERR: defmt::Format, - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - DELAY: DelayMs + DelayUs + DelayMs + DelayUs + Copy, -{ - robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY>, +> { + robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, SPI_DEV, DELAY>, delay: DELAY, led1: LED1, selected_path: Option, @@ -58,42 +48,18 @@ impl< INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI, - TWI_ERR, - SPI, - CS: OutputPin, - DELAY, + TWI: I2c, + SPI_DEV: SpiDevice, + DELAY: DelayNs + Clone, LED1: OutputPin, - > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY, LED1> -where - TWI: I2cWrite + WriteRead, - TWI_ERR: defmt::Format, - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - DELAY: DelayMs + DelayUs + DelayMs + DelayUs + Copy, + > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, SPI_DEV, DELAY, LED1> { pub fn new( - robot: Robot< - 'a, - INA1, - INA2, - INB1, - INB2, - ENA, - ENB, - BUTT1, - BUTT2, - TWI, - TWI_ERR, - SPI, - CS, - DELAY, - >, + robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, SPI_DEV, DELAY>, delay: DELAY, led1_pin: LED1, ) -> Self { @@ -111,7 +77,7 @@ where let start_millis = millis(); while millis() - start_millis < ms { self.robot.handle_loop(); - self.delay.delay_us(500_u16); + self.delay.delay_us(500); } } diff --git a/src/main.rs b/src/main.rs index 3e8a7a2..ff3584b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,45 +5,36 @@ mod model; mod robot; mod system; -use bsp::{ - entry, - hal::{fugit::HertzU32, gpio}, -}; +use core::cell::RefCell; 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. use rp_pico as bsp; // use sparkfun_pro_micro_rp2040 as bsp; - +use crate::robot::Robot; +use bsp::{ + entry, + hal::fugit::HertzU32, + hal::{ + clocks::{init_clocks_and_plls, Clock}, + gpio::{FunctionI2C, Pin, PullUp}, + pac, + pwm::Slices, + sio::Sio, + watchdog::Watchdog, + }, +}; +use driver::Driver; +use system::millis::init_millis; extern crate alloc; use embedded_alloc::TlsfHeap as Heap; - #[global_allocator] static HEAP: Heap = Heap::empty(); -use bsp::hal::{ - clocks::{init_clocks_and_plls, Clock}, - pac, - pwm::Slices, - sio::Sio, - watchdog::Watchdog, -}; - -use driver::Driver; -use robot::Robot; -use system::millis::init_millis; - #[entry] fn main() -> ! { // Initialize the allocator BEFORE you use it @@ -100,35 +91,43 @@ fn main() -> ! { channel_a.output_to(pins.gpio8); channel_b.output_to(pins.gpio9); + // Configure two pins as being I²C, not GPIO + let sda_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio4.reconfigure(); + let scl_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio5.reconfigure(); // set up I2C let i2c = bsp::hal::I2C::new_controller( pac.I2C0, - pins.gpio4.into_function::(), - pins.gpio5.into_function::(), + sda_pin, + scl_pin, HertzU32::from_raw(400_000), &mut pac.RESETS, clocks.system_clock.freq(), ); - let i2c_manager: &'static _ = shared_bus::new_cortexm!(rp2040_hal::I2C, Pin)> = i2c).unwrap(); + let i2c_ref_cell = RefCell::new(i2c); + let i2c_mutex = critical_section::Mutex::new(i2c_ref_cell); // set up SPI #[allow(clippy::type_complexity)] - let spi: rp_pico::hal::Spi< - rp_pico::hal::spi::Disabled, - SPI0, - ( - rp_pico::hal::gpio::Pin, - rp_pico::hal::gpio::Pin, - rp_pico::hal::gpio::Pin, - ), - > = bsp::hal::Spi::new( - pac.SPI0, - ( - pins.gpio3.into_function::(), - pins.gpio0.into_function::(), - pins.gpio2.into_function::(), - ), - ); + // let spi: rp_pico::hal::Spi< + // rp_pico::hal::spi::Disabled, + // SPI0, + // ( + // rp_pico::hal::gpio::Pin, + // rp_pico::hal::gpio::Pin, + // rp_pico::hal::gpio::Pin, + // ), + // > = bsp::hal::Spi::new( + // pac.SPI0, + // ( + // pins.gpio3.into_function::(), + // pins.gpio0.into_function::(), + // pins.gpio2.into_function::(), + // ), + // ); + let spi_mosi = pins.gpio3.into_function::(); + let spi_miso = pins.gpio0.into_function::(); + let spi_sclk = pins.gpio2.into_function::(); + let spi = bsp::hal::spi::Spi::<_, _, _, 8>::new(pac.SPI0, (spi_mosi, spi_miso, spi_sclk)); // Exchange the uninitialised SPI driver for an initialised one let spi = spi.init( @@ -139,16 +138,19 @@ fn main() -> ! { ); let sd = crate::robot::file_storage::FileStorage::new( - spi, - pins.gpio1.into_push_pull_output(), + crate::robot::file_storage::sd_card_spi_device::SDCardSPIDevice::new( + spi, + pins.gpio1.into_push_pull_output(), + timer, + ), timer, ); // If SD card is successfully initialized, we can increase the SPI speed match sd.spi(|spi| { - spi.set_baudrate( + spi.bus.set_baudrate( clocks.peripheral_clock.freq(), - HertzU32::from_raw(20_000_000), + HertzU32::from_raw(16_000_000), ) }) { Some(speed) => { @@ -168,7 +170,7 @@ fn main() -> ! { channel_b, pins.gpio14.into_pull_up_input(), pins.gpio15.into_pull_up_input(), - i2c_manager, + &i2c_mutex, pins.gpio21.into_pull_up_input(), pins.gpio20.into_pull_up_input(), sd, diff --git a/src/model/heading.rs b/src/model/heading.rs index 42c2cf7..dd35237 100644 --- a/src/model/heading.rs +++ b/src/model/heading.rs @@ -4,10 +4,7 @@ use crate::system::millis::millis; use core::marker::PhantomData; use defmt::{error, info}; -use embedded_hal::blocking::{ - delay::DelayMs, - i2c::{Write, WriteRead}, -}; +use embedded_hal::{delay::DelayNs, i2c::I2c}; use micromath::vector::Vector3d; use mpu6050::Mpu6050; @@ -24,55 +21,68 @@ pub struct HeadingCalculator { gyro: Mpu6050, last_update_rate: f32, last_update_millis: u32, + inited: bool, _delay: PhantomData, } #[allow(dead_code, non_camel_case_types)] -impl HeadingCalculator +impl HeadingCalculator where - TWI: Write + WriteRead, - DELAY: DelayMs, - TWI_ERR: defmt::Format, + TWI: I2c, + DELAY: DelayNs, { pub fn new(i2c: TWI, delay: &mut DELAY) -> Self { let mut gyro = Mpu6050::new(i2c); + let mut inited = false; match gyro.init(delay) { Ok(_) => { info!("Gyro initialized"); + inited = true; } - Err(e) => { - error!("Error initializing gyro: {}", e); + Err(_e) => { + error!("Error initializing gyro"); } }; - 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: {}", error); - Vector3d::::default() + if inited { + if let Err(_error) = gyro.set_gyro_range(mpu6050::device::GyroRange::D250) { + error!("Error setting gyro range"); } - }; - info!( - "Got gyro offsets: x = {}, y = {}, z = {}", - cur_offsets.x, cur_offsets.y, cur_offsets.z - ); + 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: millis(), + inited, _delay: PhantomData, } } + pub fn is_inited(&self) -> bool { + self.inited + } + pub fn calibrate(&mut self, delay: &mut DELAY, callback: F) { - if let Err(e) = self.gyro.calibrate_gyro(delay, callback) { - error!("Error calibrating gyro: {}", e); + if !self.is_inited() { + error!("Gyro not initialized"); + return; + } + if let Err(_e) = self.gyro.calibrate_gyro(delay, callback) { + error!("Error calibrating gyro"); } } @@ -83,6 +93,9 @@ where } 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 { @@ -100,8 +113,8 @@ where self.heading += 360.0; } } - Err(error) => { - error!("Error reading gyro: {}", error); + Err(_error) => { + error!("Error reading gyro"); } } } diff --git a/src/robot.rs b/src/robot.rs index 35567d0..7d10a59 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -16,7 +16,6 @@ use crate::{ }, system::millis::millis, }; -use adafruit_lcd_backpack::{LcdBackpack, LcdDisplayType}; use alloc::{ string::{String, ToString}, vec, @@ -29,13 +28,14 @@ use core::{ use cortex_m::interrupt::Mutex; use defmt::{debug, error, info, warn, Format}; use embedded_hal::{ - blocking::{ - delay::{DelayMs, DelayUs}, - i2c::{Write as I2cWrite, WriteRead}, - }, - digital::v2::{InputPin, OutputPin}, - PwmPin, + delay::DelayNs, + digital::{InputPin, OutputPin}, + i2c::I2c, + pwm::SetDutyCycle, + spi::SpiDevice, }; +use embedded_hal_bus::i2c::CriticalSectionDevice; +use i2c_character_display::{AdafruitLCDBackpack, LcdDisplayType}; use micromath::F32Ext; use rp_pico::hal::gpio; use rp_pico::hal::{ @@ -45,7 +45,6 @@ use rp_pico::hal::{ }, pac::{self, interrupt}, }; -use shared_bus::BusManagerCortexM; const UP_ARROW_CHARACTER_DEFINITION: [u8; 8] = [ 0b00000, 0b00100, 0b01110, 0b10101, 0b00100, 0b00100, 0b00100, 0b00000, @@ -96,31 +95,27 @@ pub struct Robot< INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, TWI, - TWI_ERR, - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - CS: OutputPin, + SPI_DEV: embedded_hal::spi::SpiDevice, DELAY, > where - TWI: I2cWrite + WriteRead, - TWI_ERR: defmt::Format, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - DELAY: DelayMs + DelayUs + DelayMs + DelayUs, + TWI: embedded_hal::i2c::I2c, + DELAY: DelayNs, { motors: MotorController, button1: DebouncedButton, button2: DebouncedButton, - pub heading_calculator: HeadingCalculator>>, DELAY>, - lcd: LcdBackpack>>, DELAY>, - pub sd_card: FileStorage, + pub heading_calculator: + HeadingCalculator, DELAY>, + lcd: AdafruitLCDBackpack, DELAY>, + pub sd_card: FileStorage, reset_display_start_millis: u32, log_index: u32, - pub logger: Logger, + pub logger: Logger, idle_message_line2: Option, config: Config, } @@ -132,22 +127,17 @@ impl< INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, TWI, - TWI_ERR, - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - CS: OutputPin, + SPI_DEV: embedded_hal::spi::SpiDevice, DELAY, - > Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY> + > Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, SPI_DEV, DELAY> where - TWI: I2cWrite + WriteRead, - TWI_ERR: defmt::Format, - DELAY: DelayMs + DelayUs + DelayMs + DelayUs + Copy, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, + TWI: embedded_hal::i2c::I2c, + DELAY: DelayNs + Clone, { #[allow(clippy::too_many_arguments)] pub fn new( @@ -155,18 +145,18 @@ where ina2_pin: INA2, inb1_pin: INB1, inb2_pin: INB2, - ena_pin: ENA, - enb_pin: ENB, + duty_a: ENA, + duty_b: ENB, button1_pin: BUTT1, button2_pin: BUTT2, - i2c_manager: &'a BusManagerCortexM, + i2c_refcell: &'a critical_section::Mutex>, left_counter_pin: LeftWheelCounterPin, right_counter_pin: RightWheelCounterPin, - mut sd_card: FileStorage, + mut sd_card: FileStorage, delay: &mut DELAY, ) -> Self { // create the motor controller - let motors = MotorController::new(ina1_pin, ina2_pin, inb1_pin, inb2_pin, ena_pin, enb_pin); + let motors = MotorController::new(ina1_pin, ina2_pin, inb1_pin, inb2_pin, duty_a, duty_b); // enable interrupts for wheel encoders cortex_m::interrupt::free(|cs| { @@ -183,62 +173,69 @@ where pac::NVIC::unmask(pac::Interrupt::IO_IRQ_BANK0); } - let mut lcd = LcdBackpack::new(LcdDisplayType::Lcd16x2, i2c_manager.acquire_i2c(), *delay); + // let i2c_device = embedded_hal_bus::i2c::RefCellDevice::new(i2c_refcell); + let i2c_device = embedded_hal_bus::i2c::CriticalSectionDevice::new(i2c_refcell); + let mut lcd = AdafruitLCDBackpack::new(i2c_device, LcdDisplayType::Lcd16x2, delay.clone()); match lcd.init() { Ok(_) => { info!("LCD initialized"); } - Err(e) => { - error!("Error initializing LCD: {}", e); + Err(_e) => { + error!("Error initializing LCD"); } }; - if let Err(error) = + debug!("Creating custom character 1"); + if let Err(_e) = lcd.create_char(UP_ARROW_STRING.as_bytes()[0], UP_ARROW_CHARACTER_DEFINITION) { - error!("Error creating up arrow character: {}", error); + error!("Error creating up arrow character"); }; - if let Err(error) = lcd.create_char( + if let Err(_e) = lcd.create_char( DOWN_ARROW_STRING.as_bytes()[0], DOWN_ARROW_CHARACTER_DEFINITION, ) { - error!("Error creating down arrow character: {}", error); + error!("Error creating down arrow character"); }; - if let Err(error) = - lcd.create_char(DEGREES_STRING.as_bytes()[0], DEGREES_CHARACTER_DEFINITION) + debug!("Creating custom character 2"); + if let Err(_e) = lcd.create_char(DEGREES_STRING.as_bytes()[0], DEGREES_CHARACTER_DEFINITION) { - error!("Error creating degrees character: {}", error); + error!("Error creating degrees character"); }; + debug!("LCD characters created"); - if let Err(error) = lcd + debug!("Writing to LCD first message"); + if let Err(_e) = lcd .home() - .and_then(LcdBackpack::clear) - .and_then(|lcd| LcdBackpack::print(lcd, "Calibrating Gyro")) + .and_then(AdafruitLCDBackpack::clear) + .and_then(|lcd| AdafruitLCDBackpack::print(lcd, "Calibrating Gyro")) { - error!("Error writing to LCD: {}", error); + error!("Error writing to LCD"); } - - let heading_i2c = i2c_manager.acquire_i2c(); - let mut heading_calculator = HeadingCalculator::new(heading_i2c, delay); + delay.delay_ms(5000); + let mut heading_calculator = HeadingCalculator::new( + embedded_hal_bus::i2c::CriticalSectionDevice::new(i2c_refcell), + delay, + ); heading_calculator.reset(); - if let Err(error) = lcd + if let Err(_e) = lcd .home() - .and_then(LcdBackpack::clear) - .and_then(|lcd| LcdBackpack::print(lcd, "Robot Started")) + .and_then(AdafruitLCDBackpack::clear) + .and_then(|lcd| AdafruitLCDBackpack::print(lcd, "Robot Started")) .and_then(|lcd| { write!( lcd.set_cursor(0, 1)?, "SD: {} GB", sd_card.volume_size().unwrap_or(0) / 1_073_741_824 ) - .map_err(|_e| adafruit_lcd_backpack::Error::FormattingError) + .map_err(i2c_character_display::Error::FormattingError) }) { - error!("Error writing to LCD: {}", error); + error!("Error writing to LCD"); } - let logger = Logger::new(Logger::::init_log_file(&mut sd_card)); + let logger = Logger::new(Logger::::init_log_file(&mut sd_card)); Self { motors, button1: DebouncedButton::::new(button1_pin), @@ -263,7 +260,7 @@ where Some(d) => d, None => { error!("Error opening config directory"); - return Err(embedded_sdmmc::Error::FileNotFound); + return Err(embedded_sdmmc::Error::NotFound); } }; if let Ok(mut config_file) = self.sd_card.open_file_in_dir( @@ -368,12 +365,6 @@ where cortex_m::interrupt::free(|cs| RIGHT_WHEEL_COUNTER.borrow(cs).get()) } - /// Returns a duty value normalized to the max duty of the motor. - /// The duty is clamped to the range [0, 1]. - fn noramlize_duty(&self, duty: f32) -> u16 { - (duty.clamp(0.0, 1.0) * self.motors.enable_pin_a().get_max_duty() as f32) as u16 - } - /// returns true if the button 1 is newly pressed pub fn button1_pressed(&mut self) -> bool { self.button1.is_newly_pressed() @@ -450,8 +441,8 @@ where self.heading_calculator.reset(); self.motors.set_duty( - self.noramlize_duty(self.config.straight_left_power), - self.noramlize_duty(self.config.straight_right_power), + self.config.straight_left_power, + self.config.straight_right_power, ); writeln!( @@ -461,8 +452,8 @@ where millis(), 0, 0, - 1.0, - 1.0, + 100, + 100, self.heading_calculator.heading(), 0., ), @@ -489,8 +480,8 @@ where let mut cs_indicator: &str = direction_arrow; // positive control signal means turn left, a negative control signal means turn right - let mut left_power: f32 = self.config.straight_left_power; - let mut right_power: f32 = self.config.straight_right_power; + let mut left_power: f32 = self.config.straight_left_power as f32; + let mut right_power: f32 = self.config.straight_right_power as f32; if (forward && control_signal > 0.) || (!forward && control_signal < 0.) { left_power -= control_signal; cs_indicator = LEFT_ARROW_STRING; @@ -498,10 +489,10 @@ where right_power += control_signal; cs_indicator = RIGHT_ARROW_STRING; } - self.motors.set_duty( - self.noramlize_duty(left_power), - self.noramlize_duty(right_power), - ); + left_power = left_power.clamp(0., 100.); + right_power = right_power.clamp(0., 100.); + + self.motors.set_duty(left_power as u8, right_power as u8); writeln!( self.logger, @@ -510,8 +501,8 @@ where last_update_millis, left_wheel_ticks, right_wheel_ticks, - left_power, - right_power, + left_power as u8, + right_power as u8, heading, control_signal, ), @@ -562,8 +553,8 @@ where millis(), left_wheel_ticks, right_wheel_ticks, - 0., - 0., + 0, + 0, self.heading_calculator.heading(), 0., ), @@ -619,16 +610,16 @@ where let mut stop_angle_delta: i32 = 0; if turn_degrees > 0 { self.motors.set_duty( - self.noramlize_duty(self.config.turn_left_left_power), - self.noramlize_duty(self.config.turn_left_right_power), + self.config.turn_left_left_power, + self.config.turn_left_right_power, ); self.motors.reverse_a(); self.motors.forward_b(); stop_angle_delta = self.config.turn_left_stop_angle_delta; } else { self.motors.set_duty( - self.noramlize_duty(self.config.turn_right_left_power), - self.noramlize_duty(self.config.turn_right_right_power), + self.config.turn_right_left_power, + self.config.turn_right_right_power, ); self.motors.forward_a(); self.motors.reverse_b(); @@ -687,7 +678,9 @@ where //-------------------------------------------------------------------------- // Test functions //-------------------------------------------------------------------------- - pub fn display_heading(&mut self) -> Result<(), adafruit_lcd_backpack::Error> { + pub fn display_heading( + &mut self, + ) -> Result<(), i2c_character_display::Error>> { write!(self.lcd.clear()?.set_cursor(0, 0)?, "Heading:").ok(); self.heading_calculator.reset(); let mut continue_loop = true; @@ -738,23 +731,17 @@ impl< INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, TWI, - TWI_ERR, - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - CS: OutputPin, + SPI_DEV: SpiDevice, DELAY, - > Format - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY> + > Format for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, SPI_DEV, DELAY> where - TWI: I2cWrite + WriteRead, - TWI_ERR: defmt::Format, - DELAY: DelayMs + DelayUs + DelayMs + DelayUs + Copy, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, + TWI: I2c, + DELAY: DelayNs + Clone, { fn format(&self, f: defmt::Formatter) { defmt::write!( @@ -773,23 +760,18 @@ impl< INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, TWI, - TWI_ERR, - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - CS: OutputPin, + SPI_DEV: SpiDevice, DELAY, > core::fmt::Write - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, TWI_ERR, SPI, CS, DELAY> + for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI, SPI_DEV, DELAY> where - TWI: I2cWrite + WriteRead, - TWI_ERR: defmt::Format, - DELAY: DelayMs + DelayUs + DelayMs + DelayUs + Copy, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, + TWI: I2c, + DELAY: DelayNs + Clone, { fn write_str(&mut self, s: &str) -> core::fmt::Result { self.lcd.write_str(s) diff --git a/src/robot/config.rs b/src/robot/config.rs index 5496216..116ec1b 100644 --- a/src/robot/config.rs +++ b/src/robot/config.rs @@ -6,8 +6,8 @@ const WHEEL_DIAMETER: f32 = 67.0; // mm const WHEEL_CIRCUMFERENCE: f32 = WHEEL_DIAMETER * core::f32::consts::PI; const WHEEL_TICKS_PER_MM: f32 = 3.0; -const STRAIGHT_LEFT_MOTOR_POWER: f32 = 1.0; -const STRAIGHT_RIGHT_MOTOR_POWER: f32 = 1.0; +const STRAIGHT_LEFT_MOTOR_POWER: u8 = 100; +const STRAIGHT_RIGHT_MOTOR_POWER: u8 = 100; const STRAIGHT_PID_P: f32 = 0.5; const STRAIGHT_PID_I: f32 = 0.0; const STRAIGHT_PID_D: f32 = 0.0; @@ -16,16 +16,22 @@ const DEFAULT_IDLE_MESSAGE: &str = "Robot Idle"; pub struct Config { // straight configuration - pub straight_left_power: f32, - pub straight_right_power: f32, + /// Motor power for left straight movement in integer percent (max 100) + pub straight_left_power: u8, + /// Motor power for right straight movement in integer percent (max 100) + pub straight_right_power: u8, pub straight_pid_p: f32, pub straight_pid_i: f32, pub straight_pid_d: f32, // turn configuration - pub turn_left_left_power: f32, - pub turn_left_right_power: f32, - pub turn_right_left_power: f32, - pub turn_right_right_power: f32, + /// Motor power for left turn in integer percent (max 100) + pub turn_left_left_power: u8, + /// Motor power for right turn in integer percent (max 100) + pub turn_left_right_power: u8, + /// Motor power for left turn in integer percent (max 100) + pub turn_right_left_power: u8, + /// Motor power for right turn in integer percent (max 100) + pub turn_right_right_power: u8, pub turn_left_stop_angle_delta: i32, pub turn_right_stop_angle_delta: i32, // general configuration @@ -41,10 +47,10 @@ impl Config { straight_pid_p: STRAIGHT_PID_P, straight_pid_i: STRAIGHT_PID_I, straight_pid_d: STRAIGHT_PID_D, - turn_left_left_power: 1.0, - turn_left_right_power: 1.0, - turn_right_left_power: 1.0, - turn_right_right_power: 1.0, + turn_left_left_power: 100, + turn_left_right_power: 100, + turn_right_left_power: 100, + turn_right_right_power: 100, turn_left_stop_angle_delta: 0, turn_right_stop_angle_delta: 0, wheel_ticks_per_mm: WHEEL_TICKS_PER_MM, @@ -64,16 +70,22 @@ impl Config { match key.trim() { "straight_left_power" => { if value.is_some() { - if let Ok(power) = value.unwrap().trim().parse::() { + if let Ok(power) = value.unwrap().trim().parse::() { self.straight_left_power = power; + if self.straight_left_power > 100 { + self.straight_left_power = 100; + } info!("CONFIG: left motor power = {}", power); } } } "straight_right_power" => { if value.is_some() { - if let Ok(power) = value.unwrap().trim().parse::() { + if let Ok(power) = value.unwrap().trim().parse::() { self.straight_right_power = power; + if self.straight_right_power > 100 { + self.straight_right_power = 100; + } info!("CONFIG: right motor power = {}", power); } } @@ -113,32 +125,44 @@ impl Config { } "turn_left_left_power" => { if value.is_some() { - if let Ok(power) = value.unwrap().trim().parse::() { + if let Ok(power) = value.unwrap().trim().parse::() { self.turn_left_left_power = power; + if self.turn_left_left_power > 100 { + self.turn_left_left_power = 100; + } info!("CONFIG: left motor power for left turn = {}", power); } } } "turn_left_right_power" => { if value.is_some() { - if let Ok(power) = value.unwrap().trim().parse::() { + if let Ok(power) = value.unwrap().trim().parse::() { self.turn_left_right_power = power; + if self.turn_left_right_power > 100 { + self.turn_left_right_power = 100; + } info!("CONFIG: right motor power for left turn = {}", power); } } } "turn_right_left_power" => { if value.is_some() { - if let Ok(power) = value.unwrap().trim().parse::() { + if let Ok(power) = value.unwrap().trim().parse::() { self.turn_right_left_power = power; + if self.turn_right_left_power > 100 { + self.turn_right_left_power = 100; + } info!("CONFIG: left motor power for right turn = {}", power); } } } "turn_right_right_power" => { if value.is_some() { - if let Ok(power) = value.unwrap().trim().parse::() { + if let Ok(power) = value.unwrap().trim().parse::() { self.turn_right_right_power = power; + if self.turn_right_right_power > 100 { + self.turn_right_right_power = 100; + } info!("CONFIG: right motor power for right turn = {}", power); } } diff --git a/src/robot/debouncer.rs b/src/robot/debouncer.rs index 0ab5082..8f7b15e 100644 --- a/src/robot/debouncer.rs +++ b/src/robot/debouncer.rs @@ -1,6 +1,6 @@ use crate::system::millis::millis; use defmt::trace; -use embedded_hal::digital::v2::InputPin; +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 diff --git a/src/robot/file_storage.rs b/src/robot/file_storage.rs index 2be0eb8..bf18f77 100644 --- a/src/robot/file_storage.rs +++ b/src/robot/file_storage.rs @@ -1,14 +1,14 @@ #![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}; use core::cell::RefCell; -use defmt::{debug, error, info}; -use embedded_hal::blocking::delay::DelayUs; -use embedded_hal::digital::v2::OutputPin; +use defmt::{debug, error, info, trace}; +use embedded_hal::{delay::DelayNs, spi::SpiDevice}; use embedded_sdmmc::{ - DirEntry, Directory, Mode, SdCard, SdCardError, TimeSource, Timestamp, Volume, VolumeIdx, + DirEntry, Mode, RawDirectory, RawVolume, SdCard, SdCardError, TimeSource, Timestamp, VolumeIdx, VolumeManager, }; use sd_file::SDFile; @@ -32,30 +32,24 @@ impl TimeSource for DummyTimesource { } } -pub struct FileStorage +pub struct FileStorage where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { - volume_mgr: Option, DummyTimesource>>>>, - volume: Option, - root_dir: Option, + volume_mgr: Option, DummyTimesource>>>>, + volume: Option, + root_dir: Option, } -impl FileStorage +impl FileStorage where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { - pub fn new(spi: SPI, cs: CS, delay: DELAY) -> Self { + pub fn new(spi: SPI_DEV, delay: DELAY) -> Self { info!("Initialize SPI SD/MMC data structures..."); - let sd_card = SdCard::new(spi, cs, delay); + let sd_card = SdCard::new(spi, delay); let mut volume_mgr = VolumeManager::new(sd_card, DummyTimesource::default()); // check that we can read the volumn size @@ -73,7 +67,7 @@ where } } - debug!("Getting Volume 0..."); + trace!("Getting Volume 0..."); let volume = match volume_mgr.open_volume(VolumeIdx(0)) { Ok(v) => v, Err(e) => { @@ -88,7 +82,8 @@ where // After we have the volume (partition) of the drive we got to open the // root directory: debug!("Opening root dir..."); - let root_dir = match volume_mgr.open_root_dir(volume) { + let raw_volume = volume.to_raw_volume(); + let root_dir = match volume_mgr.open_root_dir(raw_volume) { Ok(dir) => dir, Err(e) => { error!("Error opening root dir: {}", defmt::Debug2Format(&e)); @@ -103,14 +98,14 @@ where Self { volume_mgr: Some(Rc::new(RefCell::new(volume_mgr))), - volume: Some(volume), + volume: Some(raw_volume), root_dir: Some(root_dir), } } pub fn volume_manager( &self, - ) -> Option, DummyTimesource>>>> { + ) -> Option, DummyTimesource>>>> { self.volume_mgr.clone() } @@ -131,11 +126,11 @@ where } } - pub fn root_dir(&self) -> Option { + pub fn root_dir(&self) -> Option { self.root_dir } - pub fn open_directory(&self, parent: Directory, name: &str) -> Option { + pub fn open_directory(&self, parent: RawDirectory, name: &str) -> Option { match self .volume_mgr .as_ref()? @@ -152,7 +147,7 @@ where pub fn spi(&self, func: F) -> Option where - F: FnOnce(&mut SPI) -> T, + F: FnOnce(&mut SPI_DEV) -> T, { Some( self.volume_mgr @@ -167,9 +162,9 @@ where pub fn open_file_in_dir( &mut self, filename: &str, - directory: Directory, + directory: RawDirectory, mode: Mode, - ) -> Result, embedded_sdmmc::Error> { + ) -> Result, embedded_sdmmc::Error> { Ok(SDFile::new( filename, directory, @@ -180,7 +175,7 @@ where pub fn iterate_dir( &self, - dir: Directory, + dir: RawDirectory, func: F, ) -> Result<(), embedded_sdmmc::Error> where @@ -195,7 +190,7 @@ where pub fn list_files_in_dir_with_ext( &self, - parent_dir: Directory, + parent_dir: RawDirectory, target_dir: &str, ext: &str, ) -> Result, embedded_sdmmc::Error> { @@ -216,13 +211,10 @@ where } } -impl Drop for FileStorage +impl Drop for FileStorage where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { fn drop(&mut self) { info!("FileStorage dropped"); diff --git a/src/robot/file_storage/logger.rs b/src/robot/file_storage/logger.rs index b3d98e9..98c148b 100644 --- a/src/robot/file_storage/logger.rs +++ b/src/robot/file_storage/logger.rs @@ -4,7 +4,7 @@ use alloc::{ vec, }; use defmt::{debug, error, info, warn}; -use embedded_hal::{blocking::delay::DelayUs, digital::v2::OutputPin}; +use embedded_hal::{delay::DelayNs, spi::SpiDevice}; use embedded_sdmmc::SdCardError; use super::{sd_file::SDFile, FileStorage}; @@ -15,28 +15,22 @@ use super::{sd_file::SDFile, FileStorage}; const LOG_DIR_NAME: &str = "log"; const LOG_INDEX_FILE_NAME: &str = "index.txt"; -pub struct Logger +pub struct Logger where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { - log_file: Option>, + log_file: Option>, buffer: [u8; BUFSIZE], buffer_index: usize, } -impl Logger +impl Logger where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { - pub fn new(log_file: Option>) -> Self { + pub fn new(log_file: Option>) -> Self { Self { log_file, buffer: [0u8; BUFSIZE], @@ -46,8 +40,8 @@ where /// Initializes the log file and log file index pub fn init_log_file( - sdcard: &mut FileStorage, - ) -> Option> { + sdcard: &mut FileStorage, + ) -> Option> { // open log directory let root_dir = match sdcard.root_dir() { Some(dir) => dir, @@ -208,13 +202,10 @@ where } } /// Implement the `core::fmt::Write` trait for the logger -impl core::fmt::Write for Logger +impl core::fmt::Write for Logger where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { fn write_str(&mut self, s: &str) -> core::fmt::Result { self.write_to_buffer(s).map_err(|_| core::fmt::Error)?; diff --git a/src/robot/file_storage/sd_card_spi_device.rs b/src/robot/file_storage/sd_card_spi_device.rs new file mode 100644 index 0000000..4157b56 --- /dev/null +++ b/src/robot/file_storage/sd_card_spi_device.rs @@ -0,0 +1,124 @@ +use defmt::{error, trace}; +use embedded_hal::{ + delay::DelayNs, + digital::OutputPin, + spi::{ErrorType, Operation, SpiBus, SpiDevice}, +}; + +#[derive(Debug)] +pub enum SDCardSPIDeviceError { + Spi, + Pin, + 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::Pin => 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 + } + } +} + +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(()) + } + } { + // iff an error accurs, 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 b800f8e..2d82f25 100644 --- a/src/robot/file_storage/sd_file.rs +++ b/src/robot/file_storage/sd_file.rs @@ -3,37 +3,30 @@ use alloc::rc::Rc; use alloc::string::String; use core::cell::RefCell; use defmt::{debug, error}; -use embedded_hal::blocking::delay::DelayUs; -use embedded_hal::digital::v2::OutputPin; -use embedded_sdmmc::{Directory, Mode, SdCard, SdCardError, VolumeManager}; +use embedded_hal::{delay::DelayNs, spi::SpiDevice}; +use embedded_sdmmc::{Mode, RawDirectory, SdCard, SdCardError, VolumeManager}; -pub struct SDFile +pub struct SDFile where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { filename: String, - directory: Directory, + directory: RawDirectory, mode: Mode, - volume_manager: Rc, DummyTimesource>>>, + volume_manager: Rc, DummyTimesource>>>, } -impl SDFile +impl SDFile where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { pub fn new( filename: &str, - directory: Directory, + directory: RawDirectory, mode: Mode, - volume_manager: Rc, DummyTimesource>>>, + volume_manager: Rc, DummyTimesource>>>, ) -> Self { Self { filename: String::from(filename), @@ -99,19 +92,16 @@ where self.filename.as_str(), Mode::ReadWriteAppend, )?; - let write_size = self.volume_manager.borrow_mut().write(file, buf)?; + self.volume_manager.borrow_mut().write(file, buf)?; self.volume_manager.borrow_mut().close_file(file)?; - Ok(write_size) + Ok(buf.len()) } } -impl Drop for SDFile +impl Drop for SDFile where - SPI: embedded_hal::blocking::spi::Transfer + embedded_hal::blocking::spi::Write, - >::Error: core::fmt::Debug, - >::Error: core::fmt::Debug, - CS: OutputPin, - DELAY: DelayUs, + SPI_DEV: SpiDevice, + DELAY: DelayNs, { fn drop(&mut self) { if let Err(e) = self.volume_manager.borrow_mut().close_dir(self.directory) { diff --git a/src/robot/motor_controller.rs b/src/robot/motor_controller.rs index 562e991..8d0c321 100644 --- a/src/robot/motor_controller.rs +++ b/src/robot/motor_controller.rs @@ -1,5 +1,5 @@ -use embedded_hal::digital::v2::OutputPin; -use embedded_hal::PwmPin; +use embedded_hal::digital::OutputPin; +use embedded_hal::pwm::SetDutyCycle; /// A L298N motor controller that can control two motors. pub struct MotorController { @@ -9,6 +9,8 @@ pub struct MotorController { inb2: INB2, ena: ENA, enb: ENB, + duty_a: u8, + duty_b: u8, } #[allow(dead_code)] @@ -18,8 +20,8 @@ where INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, { pub fn new(ina1: INA1, ina2: INA2, inb1: INB1, inb2: INB2, ena: ENA, enb: ENB) -> Self where @@ -27,8 +29,8 @@ where INA2: OutputPin, INB1: OutputPin, INB2: OutputPin, - ENA: PwmPin, - ENB: PwmPin, + ENA: SetDutyCycle, + ENB: SetDutyCycle, { Self { ina1, @@ -37,36 +39,61 @@ where inb2, ena, enb, + duty_a: 0, + duty_b: 0, } } - pub fn enable_pin_a(&self) -> &ENA { - &self.ena + // pub fn enable_pin_a(&self) -> &ENA { + // &self.ena + // } + + // pub fn enable_pin_b(&self) -> &ENB { + // &self.enb + // } + + pub fn set_duty(&mut self, duty_percent_a: u8, duty_percent_b: u8) { + self.set_duty_a(duty_percent_a); + self.set_duty_b(duty_percent_b); + } + + pub fn set_duty_a(&mut self, duty_percent_a: u8) { + self.duty_a = duty_percent_a; + if self.duty_a > 100 { + self.duty_a = 100; + } + } + + pub fn get_duty_percent_a(&self) -> u8 { + self.duty_a } - pub fn enable_pin_b(&self) -> &ENB { - &self.enb + pub fn enable_a(&mut self) { + let _ = self.ena.set_duty_cycle_percent(self.duty_a); } - pub fn set_duty(&mut self, duty_a: ENA::Duty, duty_b: ENB::Duty) { - self.ena.set_duty(duty_a); - self.enb.set_duty(duty_b); + pub fn disable_a(&mut self) { + let _ = self.ena.set_duty_cycle_percent(0); } - pub fn set_duty_a(&mut self, duty: ENA::Duty) { - self.ena.set_duty(duty); + pub fn set_duty_b(&mut self, duty_percent_b: u8) { + self.duty_b = duty_percent_b; + if self.duty_b > 100 { + self.duty_b = 100; + } + let _ = self.enb.set_duty_cycle_percent(self.duty_b); } - pub fn set_duty_b(&mut self, duty: ENB::Duty) { - self.enb.set_duty(duty); + pub fn get_duty_percent_b(&self) -> u8 { + self.duty_b } - pub fn get_duty_a(&self) -> ENA::Duty { - self.ena.get_duty() + pub fn enable_b(&mut self) { + let _ = self.enb.set_duty_cycle_percent(self.duty_b); } - pub fn get_duty_b(&self) -> ENB::Duty { - self.enb.get_duty() + pub fn disable_b(&mut self) { + let _ = self.enb.set_duty_cycle_percent(0); } pub fn forward(&mut self) { @@ -74,20 +101,20 @@ where self.ina2.set_low().ok(); self.inb1.set_high().ok(); self.inb2.set_low().ok(); - self.ena.enable(); - self.enb.enable(); + self.enable_a(); + self.enable_b(); } pub fn forward_a(&mut self) { self.ina1.set_high().ok(); self.ina2.set_low().ok(); - self.ena.enable(); + self.enable_a(); } pub fn forward_b(&mut self) { self.inb1.set_high().ok(); self.inb2.set_low().ok(); - self.enb.enable(); + self.enable_b(); } pub fn reverse(&mut self) { @@ -95,25 +122,25 @@ where self.ina2.set_high().ok(); self.inb1.set_low().ok(); self.inb2.set_high().ok(); - self.ena.enable(); - self.enb.enable(); + self.enable_a(); + self.enable_b(); } pub fn reverse_a(&mut self) { self.ina1.set_low().ok(); self.ina2.set_high().ok(); - self.ena.enable(); + self.enable_a(); } pub fn reverse_b(&mut self) { self.inb1.set_low().ok(); self.inb2.set_high().ok(); - self.enb.enable(); + self.enable_b(); } pub fn stop(&mut self) { - self.ena.disable(); - self.enb.disable(); + self.disable_a(); + self.disable_b(); self.ina1.set_low().ok(); self.ina2.set_low().ok(); self.inb1.set_low().ok(); @@ -121,13 +148,13 @@ where } pub fn stop_a(&mut self) { - self.ena.disable(); + self.disable_a(); self.ina1.set_low().ok(); self.ina2.set_low().ok(); } pub fn stop_b(&mut self) { - self.enb.disable(); + self.disable_b(); self.inb1.set_low().ok(); self.inb2.set_low().ok(); } diff --git a/src/robot/telemetry/straight_movement.rs b/src/robot/telemetry/straight_movement.rs index 1efaca0..c57f05b 100644 --- a/src/robot/telemetry/straight_movement.rs +++ b/src/robot/telemetry/straight_movement.rs @@ -3,8 +3,8 @@ pub struct StraightTelemetryRow { time: u32, left_wheel_ticks: u32, right_wheel_ticks: u32, - left_motor_power: f32, - right_motor_power: f32, + left_motor_power: u8, + right_motor_power: u8, heading: f32, control_signal: f32, } @@ -28,8 +28,8 @@ impl StraightTelemetryRow { time: u32, left_wheel_ticks: u32, right_wheel_ticks: u32, - left_motor_power: f32, - right_motor_power: f32, + left_motor_power: u8, + right_motor_power: u8, heading: f32, control_signal: f32, ) -> Self {