From 23a014c024c97e37600fe14f1a6f16fb1297768b Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Fri, 10 May 2024 13:19:17 -0500
Subject: [PATCH] switched to pin change interrupt to use serial

---
 src/encoder.rs | 47 +++++++++++++++++++++--------------------------
 src/main.rs    | 34 ++++++++++++++++++++++++----------
 2 files changed, 45 insertions(+), 36 deletions(-)

diff --git a/src/encoder.rs b/src/encoder.rs
index 36c0e6c..904e77f 100644
--- a/src/encoder.rs
+++ b/src/encoder.rs
@@ -1,17 +1,15 @@
 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 arduino_hal::{hal::port::{PD1, PD2, PD3, PD4}, 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) {
+pub fn setup_encoder(d3: Pin<Input<Floating>, PD3>, d4: Pin<Input<Floating>, PD4>, int: EXINT) {
     interrupt::free(|cs| {
-        let _ = ENCODER.borrow(cs).set((d1, d2));
+        let _ = ENCODER.borrow(cs).set((d3, d4));
     });
 
-    //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());
+    int.pcicr.write(|w| unsafe { w.bits(0b100) });
+    int.pcmsk2.write(|w| w.bits(0b11000)); // d3 d4
+    unsafe {avr_device::interrupt::enable()}
 }
 
 pub fn rotations() -> f32 {
@@ -22,21 +20,17 @@ pub fn ticks() -> 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 ENCODER: Mutex<OnceCell<(Pin<Input<Floating>, PD3>, Pin<Input<Floating>, PD4>)>> = 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.;
+pub const TICKS_PER_ROT: f32 = 90.;
 
-#[avr_device_macros::interrupt(atmega328p)]
-fn INT0() {
-    interrupt::free(|cs| update_encoder(cs))
-}
-#[avr_device_macros::interrupt(atmega328p)]
-fn INT1() {
+#[avr_device::interrupt(atmega328p)]
+fn PCINT2() {
     interrupt::free(|cs| update_encoder(cs))
 }
 
-fn update_encoder(cs: CriticalSection) {
+pub 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;
@@ -44,16 +38,17 @@ fn update_encoder(cs: CriticalSection) {
     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,
+    let diff: i8 = match (curr, last) {
+        (0b00, 0b01) => 1,
+        (0b01, 0b11) => 1,
+        (0b11, 0b10) => 1,
+        (0b10, 0b00) => 1,
+        (0b00, 0b10) => -1,
+        (0b10, 0b11) => -1,
+        (0b11, 0b01) => -1,
+        (0b01, 0b00) => -1,
         _ => 0,
+        //_ => {(curr<<2+last) as i8},
     };
     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 a1319c6..c49fd22 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,18 +2,21 @@
 #![no_main]
 #![feature(abi_avr_interrupt, cell_update)]
 
-use arduino_hal::{hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
+use core::sync::atomic::Ordering;
+
+use arduino_hal::{default_serial, hal::port::PD4, pac::tc1::OCR1A, port::{mode::{Input, PullUp}, Pin}};
 use avr_device::interrupt;
-use encoder::{rotations, setup_encoder, COUNTS, TICKS_PER_ROT};
+use encoder::{rotations, setup_encoder, update_encoder, COUNTS, TICKS_PER_ROT};
 use panic_halt as _;
 mod servo;
 use servo::{configure_timer_one, Servo};
+use ufmt::uwriteln;
 mod encoder;
 
-// d1: encoder 
-// d2: encoder
-// d4: limit switch
-// d5: lift piston
+// d3: encoder
+// d4: encoder
+// d5: limit switch
+// d6: lift piston
 // d9: carriage
 // d10: claw
 
@@ -22,17 +25,29 @@ fn main() -> ! {
     let dp = arduino_hal::Peripherals::take().unwrap();
     let pins = arduino_hal::pins!(dp);
 
+    let mut serial = arduino_hal::Usart::new(                                        
+        dp.USART0,                                                               
+        pins.d0,                                                                 
+        pins.d1.into_output(),                                                   
+        arduino_hal::hal::usart::BaudrateArduinoExt::into_baudrate(57600),       
+    );       
+
     // configure encoder
-    setup_encoder(pins.d1, pins.d2, dp.EXINT);
+    setup_encoder(pins.d3, pins.d4, dp.EXINT);
 
     // configure timer for servos
     pins.d9.into_output(); // carriage
     pins.d10.into_output(); // claw
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
-    let switch = pins.d4.into_pull_up_input();
+    let switch = pins.d5.into_pull_up_input();
+
+    //home(&carriage, &switch);
+
+    //claw.set_speed(-0.4);
+    //move_to(&carriage, 1.);
+    //claw.set_speed(0.4);
 
-    home(&carriage, &switch);
     loop {
         let gain = rotations();
 
@@ -41,7 +56,6 @@ fn main() -> ! {
     }
 }
 
-
 const MAX_VELOCITY: f32 = 0.2; // rotations per 10ms
 const ACCEPTABLE_ERROR: f32 = 0.2; // rotations
 const KP: f32 = 0.2;