From 48785bc4172e59d179e7a191c70b83e97873eb53 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 2 May 2024 09:04:25 -0500
Subject: [PATCH] initial encoder code (depends on std, oops)

---
 src/main.rs | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 56 insertions(+), 1 deletion(-)

diff --git a/src/main.rs b/src/main.rs
index 73f0b1f..8813653 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,14 +1,28 @@
 #![cfg_attr(not(test), no_std)]
 #![no_main]
+#![feature(abi_avr_interrupt)]
 
+use core::sync::atomic::{AtomicI64, AtomicU8};
+use std::sync::OnceLock;
+
+use arduino_hal::{hal::port::{PD1, PD2}, pac::exint::eicra::{ISC0_A, ISC0_W, ISC1_A}, port::{mode::Input, Pin}};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
+#[macro_use]
+use avr_device_macros;
 
 #[arduino_hal::entry]
 fn main() -> ! {
     let dp = arduino_hal::Peripherals::take().unwrap();
     let pins = arduino_hal::pins!(dp);
+    ENCODER.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 timer for servos
     pins.d9.into_output(); // carriage
@@ -16,7 +30,48 @@ fn main() -> ! {
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
     loop {
-        carriage.set_speed(1.0);
+        let position = COUNTS.load(core::sync::atomic::Ordering::SeqCst);
+
+        let gain = position as f32 / TICKS_PER_ROT;
+
+        carriage.set_speed(gain);
         arduino_hal::delay_ms(20);
     }
 }
+
+static ENCODER: OnceLock<(Pin<Input, PD1>, Pin<Input, PD2>)> = OnceLock::new();
+static LAST: AtomicU8 = AtomicU8::new(0);
+static COUNTS: AtomicI64 = AtomicI64::new(0);
+const TICKS_PER_ROT: f32 = 90. * 4.;
+
+#[avr_device_macros::interrupt(atmega328p)]
+fn INT0() {
+    update_encoder()
+}
+#[avr_device_macros::interrupt(atmega328p)]
+fn INT1() {
+    update_encoder()
+}
+
+fn update_encoder() {
+    let (d1,d2) = ENCODER.get().unwrap();
+    let d1 = d1.is_high();
+    let d2 = d2.is_high();
+    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.fetch_add(diff as i64, core::sync::atomic::Ordering::SeqCst);
+    LAST.store(curr, core::sync::atomic::Ordering::SeqCst);
+}