Add encoder

Cargo.lock

@@ -109,6 +109,7 @@ dependencies = [
  "cortex-m-rt",
  "defmt",
  "defmt-rtt",
+ "embedded-hal 0.2.7",
  "panic-probe",
  "rtic",
  "rtic-monotonics",

Cargo.toml

@@ -9,6 +9,7 @@ cortex-m = { version = "0.7.7", features = ["critical-section-single-core"] }
 cortex-m-rt = "0.7.3"
 defmt = "0.3.5"
 defmt-rtt = "0.4.0"
+embedded-hal = "0.2.7"
 panic-probe = { version = "0.3.1", features = ["print-defmt"] }
 rtic = { version = "2.0.1", features = ["thumbv7-backend"] }
 rtic-monotonics = { version = "1.5.0", features = ["cortex-m-systick"] }

README.org

@@ -15,3 +15,7 @@
 ** Driver IC TB6612FNG for dual DC motor
 
 - [[https://www.sparkfun.com/datasheets/Robotics/TB6612FNG.pdf][Datasheet]]
+
+** Encoders FIT0450
+
+- [[https://wiki.dfrobot.com/Micro_DC_Motor_with_Encoder-SJ01_SKU__FIT0450][Wiki]]

src/encoders.rs

+use cortex_m::prelude::_embedded_hal_Qei;
+pub use embedded_hal::Direction;
+use stm32f1xx_hal::{
+    afio::MAPR,
+    gpio::{PA0, PA1, PA6, PA7},
+    pac::{TIM2, TIM3},
+    qei::{Qei, QeiOptions},
+    rcc::Clocks,
+    timer::{Tim2NoRemap, Tim3NoRemap, Timer},
+};
+
+pub struct Encoders {
+    left: Qei<TIM3, Tim3NoRemap, (PA6, PA7)>,
+    right: Qei<TIM2, Tim2NoRemap, (PA0, PA1)>,
+    count_left: u16,
+    count_right: u16,
+}
+
+#[derive(Debug)]
+pub struct EncoderValue {
+    count: u16,
+    direction: Direction,
+}
+
+impl Encoders {
+    pub fn new(
+        pa0: PA0,
+        pa1: PA1,
+        pa6: PA6,
+        pa7: PA7,
+        tim2: TIM2,
+        tim3: TIM3,
+        mapr: &mut MAPR,
+        clocks: &Clocks,
+    ) -> Self {
+        let options = QeiOptions::default();
+        let qei = Self {
+            left: Timer::new(tim3, clocks).qei((pa6, pa7), mapr, options),
+            right: Timer::new(tim2, clocks).qei((pa0, pa1), mapr, options),
+            count_left: 0,
+            count_right: 0,
+        };
+        // Unsafe needed, see <https://github.com/stm32-rs/stm32f1xx-hal/issues/478>
+        unsafe {
+            stm32f1xx_hal::pac::Peripherals::steal()
+                .TIM2
+                .ccer
+                .modify(|_, w| w.cc2p().set_bit());
+        }
+        qei
+    }
+
+    pub fn read(&self) -> (EncoderValue, EncoderValue) {
+        (
+            EncoderValue {
+                count: self.left.count(),
+                direction: self.left.direction(),
+            },
+            EncoderValue {
+                count: self.right.count(),
+                direction: self.right.direction(),
+            },
+        )
+    }
+
+    pub fn ticks(&mut self) -> (i16, i16) {
+        let (left, right) = (self.left.count(), self.right.count());
+        (
+            ticks_since(&mut self.count_left, left),
+            ticks_since(&mut self.count_right, right),
+        )
+    }
+}
+
+fn ticks_since(old: &mut u16, new: u16) -> i16 {
+    let diff = (new - *old) as i16;
+    *old = new;
+    diff
+}

src/main.rs

@@ -2,10 +2,12 @@
 #![no_main]
 #![feature(type_alias_impl_trait)]
 
+mod encoders;
 mod tb6612fng;
 
 #[rtic::app(device = pac, dispatchers = [])]
 mod app {
+    use crate::encoders::Encoders;
     use crate::tb6612fng::{Movement, Tb6612fng};
     use defmt_rtt as _;
     use panic_probe as _;
@@ -81,6 +83,18 @@ mod app {
         );
         serial.tx.bwrite_all(b"Hello, world\r\n").unwrap();
 
+        let encoders = Encoders::new(
+            gpioa.pa0,
+            gpioa.pa1,
+            gpioa.pa6,
+            gpioa.pa7,
+            dp.TIM2,
+            dp.TIM3,
+            &mut afio.mapr,
+            &clocks,
+        );
+        debug_encoders::spawn(encoders).ok().unwrap();
+
         (Shared {}, Local {})
     }
 
@@ -125,4 +139,18 @@ mod app {
         defmt::info!("Returning to standby mode");
         motors.standby_enter();
     }
+
+    #[task]
+    async fn debug_encoders(_cx: debug_encoders::Context, mut encoders: Encoders) {
+        loop {
+            let (left, right) = encoders.read();
+            defmt::info!(
+                "left = {:?}, right = {:?}",
+                defmt::Debug2Format(&left),
+                defmt::Debug2Format(&right)
+            );
+            defmt::info!("ticks = {:?}", encoders.ticks());
+            Systick::delay(100.millis()).await;
+        }
+    }
 }