diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index ffb202c..8f572af 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -22,13 +22,15 @@ fn panic() -> ! {
 mod app {
 
     use as5048a::AS5048A;
+    use embedded_hal::digital::OutputPin;
+    use rtic::Mutex;
 
-    use core::fmt::Write;
+    use core::{convert::Infallible, fmt::Write};
     use heapless::{String, Vec};
     use num_traits::{Float, FloatConst};
     use postcard::{from_bytes_cobs, to_vec_cobs};
     use stm32f4xx_hal::{
-        gpio::{gpioa, gpiob, gpioc, Output, PushPull},
+        gpio::{gpioa, gpiob, gpioc, AnyPin, Output, PushPull},
         i2c,
         otg_fs::{UsbBus, UsbBusType, USB},
         pac::{I2C1, SPI1},
@@ -76,13 +78,13 @@ mod app {
         spi_cs3: gpiob::PB15<Output<PushPull>>,
         spi1: spi::Spi<SPI1>,
 
-        az_enable: gpiob::PB8<Output<PushPull>>,
-        az_dir: gpioa::PA15<Output<PushPull>>,
-        az_step: gpiob::PB3<Output<PushPull>>,
+        az_enable: AnyPin<Output<PushPull>>,
+        az_dir: AnyPin<Output<PushPull>>,
+        az_step: AnyPin<Output<PushPull>>,
 
-        el_enable: gpiob::PB4<Output<PushPull>>,
-        el_dir: gpioa::PA8<Output<PushPull>>,
-        el_step: gpioa::PA9<Output<PushPull>>,
+        el_enable: AnyPin<Output<PushPull>>,
+        el_dir: AnyPin<Output<PushPull>>,
+        el_step: AnyPin<Output<PushPull>>,
 
         usb_dev: UsbDevice<'static, UsbBusType>,
         usb_serial: SerialPort<'static, UsbBusType>,
@@ -206,15 +208,15 @@ mod app {
 
         defmt::info!("SPI Setup done");
 
-        let mut az_enable = gpiob.pb8.into_push_pull_output();
+        let mut az_enable = gpiob.pb8.into_push_pull_output().erase();
         az_enable.set_high();
-        let az_dir = gpioa.pa15.into_push_pull_output();
-        let az_step = gpiob.pb3.into_push_pull_output();
+        let az_dir = gpioa.pa15.into_push_pull_output().erase();
+        let az_step = gpiob.pb3.into_push_pull_output().erase();
 
-        let mut el_enable = gpiob.pb4.into_push_pull_output();
+        let mut el_enable = gpiob.pb4.into_push_pull_output().erase();
         el_enable.set_high();
-        let el_dir = gpioa.pa8.into_push_pull_output();
-        let el_step = gpioa.pa9.into_push_pull_output();
+        let el_dir = gpioa.pa8.into_push_pull_output().erase();
+        let el_step = gpioa.pa9.into_push_pull_output().erase();
 
         defmt::info!("Motor Setup done");
 
@@ -368,22 +370,26 @@ mod app {
         }
     }
 
-    #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])]
-    async fn move_az(mut cx: move_az::Context) {
-        let az_enable = cx.local.az_enable;
-        let az_dir = cx.local.az_dir;
-        let az_step = cx.local.az_step;
-
+    async fn move_axis<TargetMux, AngleMux>(
+        enable: &mut AnyPin<Output<PushPull>>,
+        dir: &mut AnyPin<Output<PushPull>>,
+        step: &mut AnyPin<Output<PushPull>>,
+        target: &mut TargetMux,
+        angle: &mut AngleMux,
+    ) where
+        TargetMux: rtic::Mutex<T = i32>,
+        AngleMux: rtic::Mutex<T = i32>,
+    {
         loop {
-            let az_target = cx.shared.az_target.lock(|az_target| *az_target);
-            let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle);
-            let diff = az_angle - az_target;
+            let tmp_target = target.lock(|target| *target);
+            let tmp_angle = angle.lock(|angle| *angle);
+            let diff = tmp_angle - tmp_target;
 
             defmt::info!(
                 "angle diff/target/actual: {:?}/{:?}/{:?}",
                 diff,
-                az_target,
-                az_angle
+                tmp_target,
+                tmp_angle
             );
 
             let delay = if diff.abs() < 10 {
@@ -395,67 +401,46 @@ mod app {
             };
 
             if diff.abs() > 50 {
-                az_enable.set_low();
+                enable.set_low();
                 if diff < 0 {
-                    az_dir.set_high();
+                    dir.set_high();
                 } else {
-                    az_dir.set_low();
+                    dir.set_low();
                 }
 
-                az_step.set_low();
+                step.set_low();
                 Mono::delay(delay / 2).await;
-                az_step.set_high();
+                step.set_high();
                 Mono::delay(delay / 2).await;
             } else {
-                az_enable.set_high();
+                enable.set_high();
                 Mono::delay(delay).await;
             }
         }
     }
 
+    #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])]
+    async fn move_az(mut cx: move_az::Context) {
+        move_axis(
+            cx.local.az_enable,
+            cx.local.az_dir,
+            cx.local.az_step,
+            &mut cx.shared.az_angle,
+            &mut cx.shared.az_target,
+        )
+        .await;
+    }
+
     #[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])]
     async fn move_el(mut cx: move_el::Context) {
-        let el_enable = cx.local.el_enable;
-        let el_dir = cx.local.el_dir;
-        let el_step = cx.local.el_step;
-
-        loop {
-            let el_target = cx.shared.el_target.lock(|el_target| *el_target);
-            let el_angle = cx.shared.el_angle.lock(|el_angle| *el_angle);
-            let diff = el_angle - el_target;
-
-            defmt::info!(
-                "angle diff/target/actual: {:?}/{:?}/{:?}",
-                diff,
-                el_target,
-                el_angle
-            );
-
-            let delay = if diff.abs() < 10 {
-                10.millis()
-            } else if diff < 100 {
-                5.millis()
-            } else {
-                1.millis()
-            };
-
-            if diff.abs() > 50 {
-                el_enable.set_low();
-                if diff < 0 {
-                    el_dir.set_high();
-                } else {
-                    el_dir.set_low();
-                }
-
-                el_step.set_low();
-                Mono::delay(delay / 2).await;
-                el_step.set_high();
-                Mono::delay(delay / 2).await;
-            } else {
-                el_enable.set_high();
-                Mono::delay(delay).await;
-            }
-        }
+        move_axis(
+            cx.local.el_enable,
+            cx.local.el_dir,
+            cx.local.el_step,
+            &mut cx.shared.el_angle,
+            &mut cx.shared.el_target,
+        )
+        .await;
     }
 
     #[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])]