diff --git a/src/main.rs b/src/main.rs
index 8813653..c39fe82 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,22 +1,22 @@
 #![cfg_attr(not(test), no_std)]
 #![no_main]
-#![feature(abi_avr_interrupt)]
+#![feature(abi_avr_interrupt, cell_update)]
 
-use core::sync::atomic::{AtomicI64, AtomicU8};
-use std::sync::OnceLock;
+use core::{cell::{Cell, OnceCell}, sync::atomic::AtomicU8};
 
-use arduino_hal::{hal::port::{PD1, PD2}, pac::exint::eicra::{ISC0_A, ISC0_W, ISC1_A}, port::{mode::Input, Pin}};
+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 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));
+    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
@@ -30,7 +30,7 @@ fn main() -> ! {
     let (carriage, claw) = configure_timer_one(&dp.TC1);
 
     loop {
-        let position = COUNTS.load(core::sync::atomic::Ordering::SeqCst);
+        let position = interrupt::free(|cs| COUNTS.borrow(cs).get());
 
         let gain = position as f32 / TICKS_PER_ROT;
 
@@ -39,25 +39,25 @@ fn main() -> ! {
     }
 }
 
-static ENCODER: OnceLock<(Pin<Input, PD1>, Pin<Input, PD2>)> = OnceLock::new();
+static ENCODER: Mutex<OnceCell<(Pin<Input<Floating>, PD1>, Pin<Input<Floating>, PD2>)>> = Mutex::new(OnceCell::new());
 static LAST: AtomicU8 = AtomicU8::new(0);
-static COUNTS: AtomicI64 = AtomicI64::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() {
-    update_encoder()
+    interrupt::free(|cs| update_encoder(cs))
 }
 #[avr_device_macros::interrupt(atmega328p)]
 fn INT1() {
-    update_encoder()
+    interrupt::free(|cs| update_encoder(cs))
 }
 
-fn update_encoder() {
-    let (d1,d2) = ENCODER.get().unwrap();
-    let d1 = d1.is_high();
-    let d2 = d2.is_high();
-    let curr = d2<<1+d1;
+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?)
@@ -72,6 +72,6 @@ fn update_encoder() {
         0b0100 => -1,
         _ => 0,
     };
-    COUNTS.fetch_add(diff as i64, core::sync::atomic::Ordering::SeqCst);
+    COUNTS.borrow(cs).update(|v| v+diff as i64);
     LAST.store(curr, core::sync::atomic::Ordering::SeqCst);
 }