From df362306085f2d0984310dff1c32b4d55b18721c Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 2 May 2024 09:29:16 -0500
Subject: [PATCH] extracted encoder to module (hopefully interrupts work)

---
 src/encoder.rs | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++
 src/main.rs    | 55 +++++--------------------------------------------
 2 files changed, 61 insertions(+), 50 deletions(-)
 create mode 100644 src/encoder.rs

diff --git a/src/encoder.rs b/src/encoder.rs
new file mode 100644
index 0000000..673e2d8
--- /dev/null
+++ b/src/encoder.rs
@@ -0,0 +1,56 @@
+use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
+
+use arduino_hal::{hal::port::{PD1, PD2}, pac::{exint::eicra::{ISC0_A, ISC1_A}, EXINT}, port::{mode::{Floating, Input}, Pin}};
+use avr_device::interrupt::{self, CriticalSection, Mutex};
+pub fn setup_encoder(d1: Pin<Input<Floating>, PD1>, d2: Pin<Input<Floating>, PD2>, int: EXINT) {
+    interrupt::free(|cs| {
+        let _ = ENCODER.borrow(cs).set((d1, d2));
+    });
+
+    //dp.EXINT.pcifr.write(0b11);
+    int.eicra.write(|w| w.isc0().bits(ISC0_A::VAL_0X01 as u8)); // rising and falling edge
+    int.eimsk.write(|w| w.int0().set_bit());
+    int.eicra.write(|w| w.isc1().bits(ISC1_A::VAL_0X01 as u8)); // rising and falling edge
+    int.eimsk.write(|w| w.int1().set_bit());
+}
+
+pub fn position() -> i64{
+    interrupt::free(|cs| COUNTS.borrow(cs).get())
+}
+
+static ENCODER: Mutex<OnceCell<(Pin<Input<Floating>, PD1>, Pin<Input<Floating>, PD2>)>> = Mutex::new(OnceCell::new());
+static LAST: AtomicU8 = AtomicU8::new(0);
+pub static COUNTS: Mutex<Cell<i64>> = Mutex::new(Cell::new(0));
+pub const TICKS_PER_ROT: f32 = 90. * 4.;
+
+#[avr_device_macros::interrupt(atmega328p)]
+fn INT0() {
+    interrupt::free(|cs| update_encoder(cs))
+}
+#[avr_device_macros::interrupt(atmega328p)]
+fn INT1() {
+    interrupt::free(|cs| update_encoder(cs))
+}
+
+fn update_encoder(cs: CriticalSection) {
+    let (d1,d2) = ENCODER.borrow(cs).get().unwrap();
+    let d1 = d1.is_high() as u8;
+    let d2 = d2.is_high() as u8;
+    let curr = d2<<1 +d1;
+    let last = LAST.load(core::sync::atomic::Ordering::SeqCst);
+
+    // could probably do this bitwise (subtract individual phases?)
+    let diff: i8 = match curr<<2 +last {
+        0b0001 => 1,
+        0b0111 => 1,
+        0b1110 => 1,
+        0b1000 => 1,
+        0b0010 => -1,
+        0b1011 => -1,
+        0b1101 => -1,
+        0b0100 => -1,
+        _ => 0,
+    };
+    COUNTS.borrow(cs).update(|v| v+diff as i64);
+    LAST.store(curr, core::sync::atomic::Ordering::SeqCst);
+}
diff --git a/src/main.rs b/src/main.rs
index c39fe82..5cfdd75 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,27 +2,19 @@
 #![no_main]
 #![feature(abi_avr_interrupt, cell_update)]
 
-use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
-
-use arduino_hal::{hal::port::{PD1, PD2}, pac::exint::eicra::{ISC0_A, ISC1_A}, port::{mode::{Floating, Input}, Pin}};
-use avr_device::interrupt::{self, CriticalSection, Mutex};
+use encoder::{setup_encoder, TICKS_PER_ROT};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
+mod encoder;
 
 #[arduino_hal::entry]
 fn main() -> ! {
     let dp = arduino_hal::Peripherals::take().unwrap();
     let pins = arduino_hal::pins!(dp);
-    interrupt::free(|cs| {
-        let _ = ENCODER.borrow(cs).set((pins.d1, pins.d2));
-    });
 
-    //dp.EXINT.pcifr.write(0b11);
-    dp.EXINT.eicra.write(|w| w.isc0().bits(ISC0_A::VAL_0X01 as u8)); // rising and falling edge
-    dp.EXINT.eimsk.write(|w| w.int0().set_bit());
-    dp.EXINT.eicra.write(|w| w.isc1().bits(ISC1_A::VAL_0X01 as u8)); // rising and falling edge
-    dp.EXINT.eimsk.write(|w| w.int1().set_bit());
+    // configure encoder
+    setup_encoder(pins.d1, pins.d2, dp.EXINT);
 
     // configure timer for servos
     pins.d9.into_output(); // carriage
@@ -30,7 +22,7 @@ fn main() -> ! {
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
     loop {
-        let position = interrupt::free(|cs| COUNTS.borrow(cs).get());
+        let position = encoder::position();
 
         let gain = position as f32 / TICKS_PER_ROT;
 
@@ -38,40 +30,3 @@ fn main() -> ! {
         arduino_hal::delay_ms(20);
     }
 }
-
-static ENCODER: Mutex<OnceCell<(Pin<Input<Floating>, PD1>, Pin<Input<Floating>, PD2>)>> = Mutex::new(OnceCell::new());
-static LAST: AtomicU8 = AtomicU8::new(0);
-static COUNTS: Mutex<Cell<i64>> = Mutex::new(Cell::new(0));
-const TICKS_PER_ROT: f32 = 90. * 4.;
-
-#[avr_device_macros::interrupt(atmega328p)]
-fn INT0() {
-    interrupt::free(|cs| update_encoder(cs))
-}
-#[avr_device_macros::interrupt(atmega328p)]
-fn INT1() {
-    interrupt::free(|cs| update_encoder(cs))
-}
-
-fn update_encoder(cs: CriticalSection) {
-    let (d1,d2) = ENCODER.borrow(cs).get().unwrap();
-    let d1 = d1.is_high() as u8;
-    let d2 = d2.is_high() as u8;
-    let curr = d2<<1 +d1;
-    let last = LAST.load(core::sync::atomic::Ordering::SeqCst);
-
-    // could probably do this bitwise (subtract individual phases?)
-    let diff: i8 = match curr<<2 +last {
-        0b0001 => 1,
-        0b0111 => 1,
-        0b1110 => 1,
-        0b1000 => 1,
-        0b0010 => -1,
-        0b1011 => -1,
-        0b1101 => -1,
-        0b0100 => -1,
-        _ => 0,
-    };
-    COUNTS.borrow(cs).update(|v| v+diff as i64);
-    LAST.store(curr, core::sync::atomic::Ordering::SeqCst);
-}