diff --git a/src/movement.rs b/src/movement.rs
index 0a33d4d..a246051 100644
--- a/src/movement.rs
+++ b/src/movement.rs
@@ -17,13 +17,17 @@ const AZ_MIN_READING: f32 = 0.0;
 const AZ_MAX_READING: f32 = 4096.0;
 // Range of motion for azimuth (0 to AZ_RANGE)
 const AZ_RANGE: f32 = 360.0;
+// Tolerance for the azimuth setpoint
+const AZ_SLOP : i16 = 1;
 
 // ADC reading for elevation 0°
 const EL_MIN_READING: f32 = 0.0;
 // ADC reading for elevation 360°
 const EL_MAX_READING: f32 = 4096.0;
 // Range of motion for elevantion (0 to EL_RANGE)
-const EL_RANGE: f32 = 90.0;
+const EL_RANGE: f32 = 180.0;
+// Tolerance for the elevation setpoint
+const EL_SLOP : i16 = 1;
 
 #[embassy_executor::task]
 pub async fn movement_task(
@@ -105,13 +109,16 @@ pub async fn movement_task(
                 rotor_state.actual_pos.az = az_actual as u16;
                 rotor_state.actual_pos.el = el_actual as u16;
 
-                if !rotor_state.stopped && rotor_state.actual_pos.az < rotor_state.setpoint_pos.az {
+                let delta_az =
+                    rotor_state.setpoint_pos.az as i16 - rotor_state.actual_pos.az as i16;
+                let delta_el =
+                    rotor_state.setpoint_pos.el as i16 - rotor_state.actual_pos.el as i16;
+
+                if !rotor_state.stopped && delta_az > AZ_SLOP {
                     // Azimuth needs to move clockwise
                     cw_pin.set_high();
                     ccw_pin.set_low();
-                } else if !rotor_state.stopped
-                    && rotor_state.actual_pos.az > rotor_state.setpoint_pos.az
-                {
+                } else if !rotor_state.stopped && delta_az < -AZ_SLOP {
                     // Azimuth needs to move counter clockwise
                     cw_pin.set_low();
                     ccw_pin.set_high();
@@ -121,13 +128,11 @@ pub async fn movement_task(
                     ccw_pin.set_low();
                 }
 
-                if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el {
+                if !rotor_state.stopped && delta_el > EL_SLOP {
                     // Elevation needs to move up
                     up_pin.set_high();
                     down_pin.set_low();
-                } else if !rotor_state.stopped
-                    && rotor_state.actual_pos.el > rotor_state.setpoint_pos.el
-                {
+                } else if !rotor_state.stopped && delta_el < -EL_SLOP {
                     // Elevation needs to move down
                     up_pin.set_low();
                     down_pin.set_high();
@@ -155,14 +160,14 @@ pub async fn movement_task(
 // Simple sliding average filter
 struct Average {
     pos: usize,
-    data: Vec<f32, 10>,
+    data: Vec<f32, 5>,
 }
 
 impl Average {
     // Create a new filter and prefill the state using an initial value
     fn new(initial: f32) -> Average {
-        let mut data: Vec<f32, 10> = Vec::new();
-        data.resize(10, initial).unwrap();
+        let mut data: Vec<f32, 5> = Vec::new();
+        data.resize(5, initial).unwrap();
         Average { pos: 0, data }
     }
 
diff --git a/src/usb.rs b/src/usb.rs
index 33771b6..27892e6 100644
--- a/src/usb.rs
+++ b/src/usb.rs
@@ -123,7 +123,7 @@ pub async fn usb_task(
                         Gs232Cmd::GetEl => {
                             uwrite!(&mut resp, "EL={}\r", current_pos.el).unwrap();
                         }
-                        // Get Azimuth and Elevation. Respond with last known pair 
+                        // Get Azimuth and Elevation. Respond with last known pair
                         Gs232Cmd::GetAzEl => {
                             uwrite!(&mut resp, "AZ={} EL={}\r", current_pos.az, current_pos.el)
                                 .unwrap();
@@ -144,7 +144,7 @@ pub async fn usb_task(
                             resp.push_str("Unkown command!\r").unwrap();
                         }
                     }
-                    
+
                     // Write the response back via USB
                     match class.write_packet(resp.as_bytes()).await {
                         Ok(_) => {}