diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 4c94819..66aeb8f 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -12,7 +12,7 @@ env: jobs: build: - runs-on: ubuntu-latest + runs-on: ubuntu:22.04 steps: - name: Checkout code diff --git a/Cargo.toml b/Cargo.toml index dfc4d90..a3b0b5b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,6 +6,7 @@ members = [ "libs/engine", "libs/timer_based_buzzer_interface", "libs/hal_button", + "libs/hal_encoder_stm32f1xx", "apps/hello_world", ] diff --git a/apps/hello_world/src/main.rs b/apps/hello_world/src/main.rs index 9ed2f47..a4ec0f1 100644 --- a/apps/hello_world/src/main.rs +++ b/apps/hello_world/src/main.rs @@ -10,6 +10,7 @@ use mightybuga_bsc::prelude::*; use mightybuga_bsc::timer::SysDelay; use mightybuga_bsc::timer_based_buzzer::TimerBasedBuzzer; use mightybuga_bsc::timer_based_buzzer::TimerBasedBuzzerInterface; +use mightybuga_bsc::EncoderController; use engine::engine::EngineController; @@ -26,7 +27,7 @@ use logging::Logger; #[entry] fn main() -> ! { - let board = board::Mightybuga_BSC::take().unwrap(); + let mut board = board::Mightybuga_BSC::take().unwrap(); let mut delay = board.delay; let mut uart = board.serial; let mut led_d1 = board.led_d1; @@ -34,14 +35,6 @@ fn main() -> ! { let mut buzzer = board.buzzer; let mut engine = board.engine; - // Initialize the allocator BEFORE you use it - { - use core::mem::MaybeUninit; - const HEAP_SIZE: usize = 1024; - static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; - unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } - } - let mut logger = Logger::new(&mut uart.tx); loop { @@ -72,6 +65,14 @@ fn main() -> ! { // Turn off the LED D1 led_d2.set_low(); } + b'6' => { + // Read the left encoder + read_encoder(&mut board.encoder_l, &mut logger, &mut delay); + } + b'7' => { + // Read the right encoder + read_encoder(&mut board.encoder_r, &mut logger, &mut delay); + } b'a' => { // Move the robot forward engine.forward(u16::MAX / 4); @@ -117,6 +118,8 @@ fn print_menu(logger: &mut Logger) { logger.log(" 3. Turn off the LED D1\r\n"); logger.log(" 4. Turn on the LED D2\r\n"); logger.log(" 5. Turn off the LED D2\r\n"); + logger.log(" 6. Read the left encoder\r\n"); + logger.log(" 7. Read the right encoder\r\n"); logger.log(" a. Move the robot forward\r\n"); logger.log(" s. Move the robot backward\r\n"); logger.log(" d. Turn the robot right\r\n"); @@ -158,3 +161,51 @@ fn play_notes(logger: &mut Logger, buzzer: &mut TimerBasedBuzzer, delay: &mut Sy delay.delay(300.millis()); buzzer.turn_off(); } + +fn read_encoder(encoder: &mut mightybuga_bsc::IncrementalEncoder, logger: &mut Logger, delay: &mut SysDelay) { + encoder.enable(); + logger.log("move the encoder! (and reset MCU to exit)\r\n"); + + let mut last = 0; + loop { + let (delta, steps) = encoder.delta(); + if last != steps{ + logger.log("(steps,delta): ("); + print_number(steps, logger); + logger.log(","); + print_number(delta, logger); + logger.log(")\r\n"); + last = steps; + } + + // don't burn the CPU + delay.delay(20.millis()); + } +} + +fn print_number(n: isize, logger: &mut Logger) { + let mut len = 0; + let mut digits = [0_u8; 20]; + let mut d = 0; + let mut binary = n.abs(); + while binary > 0 { + digits[d] = (binary % 10) as u8; + d += 1; + binary /= 10; + } + if d == 0 { + d = 1; + } + let mut ascii = [0_u8; 20]; + if n < 0 { + ascii[0] = b'-'; + len = 1; + } + while d > 0 { + d -= 1; + ascii[len] = digits[d] + b'0'; + len += 1; + } + let s = core::str::from_utf8(ascii[0..len].as_ref()).unwrap(); + logger.log(s); +} \ No newline at end of file diff --git a/libs/hal_encoder/Cargo.toml b/libs/hal_encoder/Cargo.toml new file mode 100644 index 0000000..cbf1603 --- /dev/null +++ b/libs/hal_encoder/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "hal-encoder" +description = "Abstractions for an encoder controller" +version = "0.1.0" +authors = ["Rafa Couto "] +edition = "2021" + +[dependencies] diff --git a/libs/hal_encoder/src/lib.rs b/libs/hal_encoder/src/lib.rs new file mode 100644 index 0000000..48b41c8 --- /dev/null +++ b/libs/hal_encoder/src/lib.rs @@ -0,0 +1,34 @@ +#![no_std] + +pub trait EncoderController { + // This function returns the absolute step count. + fn steps(&self) -> usize; + + // This function resets the step count to zero. + fn reset(&mut self); + + // This function returns a mutable reference to store the last step count. + // It is implemented to save the state and calculate the delta. + fn last_steps_ref(&mut self) -> &mut usize; + + // MSB_MASK is used to detect overflow and underflow when the most significant bit changes. + const MSB_MASK : usize = 1 << (BITS - 1); + + // This function returns the delta of the step count since the last time this function was called. + fn delta(&mut self) -> (isize, isize) { + let steps = self.steps(); + let last_steps = self.last_steps_ref(); + let mut delta = steps as isize - *last_steps as isize; + if steps & Self::MSB_MASK != *last_steps & Self::MSB_MASK { + delta += match steps > *last_steps { + true => -(1 << BITS), // underflow + false => 1 << BITS, // overflow + } + }; + *last_steps = steps; + (delta, steps as isize) + } +} + +#[cfg(test)] +mod tests; diff --git a/libs/hal_encoder/src/tests.rs b/libs/hal_encoder/src/tests.rs new file mode 100644 index 0000000..9e45765 --- /dev/null +++ b/libs/hal_encoder/src/tests.rs @@ -0,0 +1,163 @@ +use super::*; + +struct MockEncoder { + pub steps: usize, + pub last_steps: usize, +} + +impl MockEncoder { + pub fn get_max_val(&self) -> usize { + (1 << BITS) - 1 + } + + pub fn get_min_val(&self) -> usize { + 0 + } + + pub fn simulate_move(&mut self, from: usize, to: usize) { + self.last_steps = from; + self.steps = to; + } +} + +impl Default for MockEncoder { + fn default() -> Self { + Self { + steps: 0, + last_steps: 0, + } + } +} + +impl EncoderController for MockEncoder { + fn steps(&self) -> usize { + self.steps + } + + fn last_steps_ref(&mut self) -> &mut usize { + &mut self.last_steps + } + + fn reset(&mut self) { + self.steps = 0; + self.last_steps = 0; + } +} + +#[test] +fn delta_16bit() { + // initial state + let mut encoder: MockEncoder<16> = Default::default(); + + // no movement + assert_eq!(0, encoder.delta().0); + + // advance 1 step + encoder.steps = 1; + assert_eq!(1, encoder.delta().0); + + // advance 10 steps + encoder.steps = 11; + assert_eq!(10, encoder.delta().0); + + // back 5 steps + encoder.steps = 6; + assert_eq!(-5, encoder.delta().0); +} + +#[test] +fn delta_10bit() { + // initial state + let mut encoder: MockEncoder<10> = Default::default(); + + // no movement + assert_eq!(0, encoder.delta().0); + + // advance 1 step + encoder.steps = 1; + assert_eq!(1, encoder.delta().0); + + // advance 10 steps + encoder.steps = 11; + assert_eq!(10, encoder.delta().0); + + // back 5 steps + encoder.steps = 6; + assert_eq!(-5, encoder.delta().0); +} + +#[test] +fn overflow_16bit() { + let mut encoder: MockEncoder<16> = Default::default(); + + // max and min values for 16-bit encoder + let min = encoder.get_min_val(); + assert_eq!(0, min); + let max = encoder.get_max_val(); + assert_eq!(65535, max); + + // overflow step with 16-bit encoder + encoder.simulate_move(max, min); + assert_eq!(1, encoder.delta().0); + + // overflow with 16-bit encoder (> 1 step) + encoder.simulate_move(max - 10, min + 10); + assert_eq!(21, encoder.delta().0); +} + +#[test] +fn overflow_10bit() { + let mut encoder: MockEncoder<10> = Default::default(); + + // max and min values for 10-bit encoder + let min = encoder.get_min_val(); + assert_eq!(0, min); + let max = encoder.get_max_val(); + assert_eq!(1023, max); + + // overflow with 10-bit encoder + encoder.simulate_move(max, min); + assert_eq!(1, encoder.delta().0); + + // overflow with 10-bit encoder (> 1 step) + encoder.simulate_move(max - 10, min + 10); + assert_eq!(21, encoder.delta().0); +} + +#[test] +fn underflow_16bit() { + let mut encoder: MockEncoder<16> = Default::default(); + + // max and min values for 16-bit encoder + let min = encoder.get_min_val(); + assert_eq!(0, min); + let max = encoder.get_max_val(); + assert_eq!(65535, max); + + // underflow with 16-bit encoder (1 step) + encoder.simulate_move(min, max); + assert_eq!(-1, encoder.delta().0); + + // underflow with 16-bit encoder (> 1 step) + encoder.simulate_move(min + 10, max - 10); + assert_eq!(-21, encoder.delta().0); +} + +#[test] +fn underflow_10bit() { + let mut encoder: MockEncoder<10> = Default::default(); + + // max and min values for 10-bit encoder + let min = encoder.get_min_val(); + assert_eq!(0, min); + let max = encoder.get_max_val(); + assert_eq!(1023, max); + + // underflow with 10-bit encoder (1 step) + encoder.simulate_move(min, max); + assert_eq!(-1, encoder.delta().0); + + // underflow with 10-bit encoder (> 1 step) + encoder.simulate_move(min + 10, max - 10); + assert_eq!(-21, encoder.delta().0); +} diff --git a/libs/hal_encoder_stm32f1xx/Cargo.toml b/libs/hal_encoder_stm32f1xx/Cargo.toml new file mode 100644 index 0000000..00662cc --- /dev/null +++ b/libs/hal_encoder_stm32f1xx/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "hal-encoder-stm32f1xx" +description = "Implementations of hal_encoder abstraction for STM32F1xx" +version = "0.1.0" +authors = ["Rafa Couto "] +edition = "2021" + +[dependencies] +hal-encoder = { version = "0.1.0", path = "../hal_encoder" } +stm32f1xx-hal = { version = "0.10.0", features = ["rt", "stm32f103", "medium"] } diff --git a/libs/hal_encoder_stm32f1xx/src/lib.rs b/libs/hal_encoder_stm32f1xx/src/lib.rs new file mode 100644 index 0000000..db131ec --- /dev/null +++ b/libs/hal_encoder_stm32f1xx/src/lib.rs @@ -0,0 +1,14 @@ +// This crate is a library to manage the Quadrature Encoder interface of STM32F1xx MCUs +// The timers supported are TIM2 to TIM5 (General-purpose timers) and TIM1 and TIM8 (Advanced-control timers). +// Each timer has 4 channels, so the library supports up to 2 encoders by timer since a quadrature encoder uses 2 channels. + +#![no_std] + +// re-export the HAL +pub use hal_encoder::EncoderController; + +// General-purpose timers (TIM2 to TIM5) +pub mod tim2_to_tim5; + +// Advanced-control timers (TIM1 and TIM8) +// to-do \ No newline at end of file diff --git a/libs/hal_encoder_stm32f1xx/src/tim2_to_tim5.rs b/libs/hal_encoder_stm32f1xx/src/tim2_to_tim5.rs new file mode 100644 index 0000000..6c2bf2c --- /dev/null +++ b/libs/hal_encoder_stm32f1xx/src/tim2_to_tim5.rs @@ -0,0 +1,102 @@ +pub use hal_encoder::EncoderController; +use stm32f1xx_hal::pac::tim2; + +pub struct IncrementalEncoder { + tim: *const tim2::RegisterBlock, + last_steps: usize, +} + +pub enum TimerChannels { + Ch1Ch2, + Ch3Ch4, +} + +pub enum EncoderPolarity { + PolarityAB, + PolarityBA, +} + +impl IncrementalEncoder { + const SLAVE_MODE_SELECTION: u8 = tim2::smcr::SMS_A::EncoderMode3 as u8; + + pub fn new(tim: *const tim2::RegisterBlock, channels: TimerChannels, polarity: EncoderPolarity) -> Self { + unsafe { + // up/down on TI1FP1+TI2FP2 edges depending on complementary input + (*tim).smcr.modify(|_, w| w.sms().bits(IncrementalEncoder::SLAVE_MODE_SELECTION)); + + // quadrature encoder mode, input capture channels + match channels { + TimerChannels::Ch1Ch2 => { + (*tim).ccmr1_input().modify(|_, w| w.cc1s().ti1().cc2s().ti2()); + } + TimerChannels::Ch3Ch4 => { + (*tim).ccmr2_input().modify(|_, w| w.cc3s().ti3().cc4s().ti4()); + } + } + + // polarity of the input channels + match polarity { + EncoderPolarity::PolarityAB => { + (*tim).ccer.modify(|_, w| w.cc1p().clear_bit().cc2p().clear_bit()); + } + EncoderPolarity::PolarityBA => { + (*tim).ccer.modify(|_, w| w.cc1p().set_bit().cc2p().clear_bit()); + } + } + + // initial value to the middle + (*tim).cnt.modify(|_, w| w.bits(0)); + + // auto-reload value to the maximum + (*tim).arr.modify(|_, w| w.bits(u16::MAX as u32)); + } + + Self { tim, last_steps: 0 } + } + + // return the current number of steps + pub fn get_steps(&self) -> u16 { + unsafe { + // read the counter register + (*self.tim).cnt.read().cnt().bits() as u16 + } + } + + // set the current number of steps + pub fn set_steps(&mut self, steps: u16) { + unsafe { + // set the counter register + (*self.tim).cnt.write(|w| w.bits(steps as u32)); + } + } + + // enable the encoder + pub fn enable(&mut self) { + unsafe { + // set the counter enable bit + (*self.tim).cr1.modify(|_, w| w.cen().set_bit()); + } + } + + // disable the encoder + pub fn disable(&mut self) { + unsafe { + // clear the counter enable bit + (*self.tim).cr1.modify(|_, w| w.cen().clear_bit()); + } + } +} + +impl EncoderController<16> for IncrementalEncoder { + fn steps(&self) -> usize { + self.get_steps() as usize + } + + fn reset(&mut self) { + self.set_steps(0) + } + + fn last_steps_ref(&mut self) -> &mut usize { + &mut self.last_steps + } +} diff --git a/mightybuga_bsc/Cargo.toml b/mightybuga_bsc/Cargo.toml index 06baedc..6919c2b 100644 --- a/mightybuga_bsc/Cargo.toml +++ b/mightybuga_bsc/Cargo.toml @@ -21,6 +21,7 @@ nb = "1.1.0" engine = { path = "../libs/engine/" } hal_button = { path = "../libs/hal_button" } +hal-encoder-stm32f1xx = { path = "../libs/hal_encoder_stm32f1xx" } timer_based_buzzer_interface = { path = "../libs/timer_based_buzzer_interface/" } light_sensor_array_controller = { path = "../libs/light_sensor_array_controller/" } diff --git a/mightybuga_bsc/src/lib.rs b/mightybuga_bsc/src/lib.rs index a0fb2fd..879b3c0 100644 --- a/mightybuga_bsc/src/lib.rs +++ b/mightybuga_bsc/src/lib.rs @@ -6,13 +6,14 @@ pub use stm32f1xx_hal as hal; use hal::adc::Adc; -use hal::pac::*; use hal::gpio::PullDown; +use hal::pac::*; use hal::prelude::*; use hal::serial::*; use hal::timer::SysDelay; use core::cell::RefCell; +use core::ops::Deref; use engine::engine::Engine; use engine::motor::Motor; @@ -26,6 +27,8 @@ pub use crate::hal::*; pub mod timer_based_buzzer; use timer_based_buzzer::TimerBasedBuzzer; +pub use hal_encoder_stm32f1xx::tim2_to_tim5::*; + pub mod prelude { pub use cortex_m_rt::entry; pub use stm32f1xx_hal::prelude::{ @@ -62,10 +65,13 @@ pub struct Mightybuga_BSC { PwmChannel, >, >, + // Buttons pub btn_1: hal_button::Button>, false>, pub btn_2: hal_button::Button>, false>, pub btn_3: hal_button::Button>, false>, - + // Encoders + pub encoder_r: IncrementalEncoder, + pub encoder_l: IncrementalEncoder, // Light sensor array pub light_sensor_array: LightSensorArray, } @@ -77,15 +83,15 @@ impl Mightybuga_BSC { // HAL structs let mut flash = dp.FLASH.constrain(); - // We need to enable the clocks here for the peripherals we want to use because the - // `constrain` frees the `RCC` register proxy. - - // Enable the timer 3 clock in the RCC register (we need to do this before the constrain) - dp.RCC.apb1enr.modify(|_, w| w.tim3en().set_bit()); - - let rcc = dp.RCC.constrain(); + // reset and clock control + let rcc = dp.RCC; + rcc.apb1enr.modify(|_, w| w.tim2en().set_bit()); + rcc.apb1enr.modify(|_, w| w.tim3en().set_bit()); + rcc.apb1enr.modify(|_, w| w.tim4en().set_bit()); + // Use external crystal for clock let clocks = rcc + .constrain() .cfgr .use_hse(8.MHz()) .sysclk(72.MHz()) @@ -95,13 +101,15 @@ impl Mightybuga_BSC { let mut delay = cp.SYST.delay(&clocks); delay.delay(300.millis()); - let mut afio = dp.AFIO.constrain(); - // GPIO ports let mut gpioa = dp.GPIOA.split(); let mut gpiob = dp.GPIOB.split(); let mut gpioc = dp.GPIOC.split(); + // Alternate function I/O remapping + let mut afio = dp.AFIO.constrain(); + let (_pa15, _pb3, pb4) = afio.mapr.disable_jtag(gpioa.pa15, gpiob.pb3, gpiob.pb4); + // LEDs configuration let d1 = gpioc.pc13.into_push_pull_output(&mut gpioc.crh); let d2 = gpiob.pb12.into_push_pull_output(&mut gpiob.crh); @@ -151,8 +159,6 @@ impl Mightybuga_BSC { let engine = Engine::new(motor_left, motor_right); // Buzzer configuration - let (_pa15, _pb3, pb4) = afio.mapr.disable_jtag(gpioa.pa15, gpiob.pb3, gpiob.pb4); - // Remap TIM3 gpio pin afio.mapr .modify_mapr(|_, w| unsafe { w.tim3_remap().bits(0b10) }); let buzzer_pin = pb4.into_alternate_push_pull(&mut gpiob.crl); @@ -163,6 +169,22 @@ impl Mightybuga_BSC { let btn_2 = hal_button::Button::new(gpioc.pc15.into_pull_down_input(&mut gpioc.crh)); let btn_3 = hal_button::Button::new(gpioc.pc14.into_pull_down_input(&mut gpioc.crh)); + // Encoder right + let encoder_r = IncrementalEncoder::new( + dp.TIM4.deref(), + TimerChannels::Ch1Ch2, + EncoderPolarity::PolarityBA, + ); + + // Encoder left + afio.mapr + .modify_mapr(|_, w| unsafe { w.tim2_remap().bits(0b01) }); + let encoder_l = IncrementalEncoder::new( + dp.TIM2.deref(), + TimerChannels::Ch1Ch2, + EncoderPolarity::PolarityBA, + ); + // Initialize the line sensor array let adc1 = Adc::adc1(dp.ADC1, clocks); let light_sensor_array = LightSensorArray { @@ -186,6 +208,8 @@ impl Mightybuga_BSC { delay, buzzer, engine, + encoder_r, + encoder_l, btn_1, btn_2, btn_3,