diff --git a/src/config/config.h b/src/config/config.h
index 82f557be..70403a91 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -115,7 +115,7 @@
 
 // PID settings
 // ! At this time, this feature is still under development
-#define ENABLE_PID
+//#define ENABLE_PID
 #ifdef ENABLE_PID
 
     // Default P, I, and D terms
@@ -137,7 +137,7 @@
 #endif
 
 // Direct step functionality (used to command motor to move over Serial/CAN)
-#define ENABLE_DIRECT_STEPPING
+//#define ENABLE_DIRECT_STEPPING
 #ifdef ENABLE_DIRECT_STEPPING
 
     // The default stepping rate (in Hz) to move in the event that no parameter is specified
diff --git a/src/config/config_adv.h b/src/config/config_adv.h
index f4bdd21e..f2027a82 100644
--- a/src/config/config_adv.h
+++ b/src/config/config_adv.h
@@ -22,7 +22,8 @@ typedef float real_t;
 #define REJECT_ENCODERS_LSB 2
 
 // If the steps should be counted using a hardware counter
-#define USE_HARDWARE_STEP_CNT
+//#define USE_HARDWARE_STEP_CNT
+#define USE_SOFTWARE_STEP_CNT
 
 // Board characteristics
 // ! Do not modify unless you know what you are doing!
@@ -43,7 +44,7 @@ typedef float real_t;
 // This can be set to 72 and 128 with SYSCLK_SRC_HSE_8 (external oscillator)
 // Can be set to 72 with SYSCLK_SRC_HSE_16 (external oscillator)
 // Can be set to 64 with SYSCLK_SRC_HSI (internal oscillator)
-#define SYSCLK_FREQ 128
+#define SYSCLK_FREQ 72
 #define SYSCLK_SRC_HSE_8
 
 // The compare format and maximum value for PWM (lower values = higher max freq)
@@ -122,7 +123,7 @@ typedef float real_t;
 
 // LED related debugging
 #ifdef ENABLE_LED
-    //#define CHECK_STEPPING_RATE
+    #define CHECK_STEPPING_RATE
     //#define CHECK_CORRECT_MOTOR_RATE
     //#define CHECK_ENCODER_SPEED
 #endif
diff --git a/src/hardware/motor.cpp b/src/hardware/motor.cpp
index 10dbc577..87c8ae5e 100644
--- a/src/hardware/motor.cpp
+++ b/src/hardware/motor.cpp
@@ -150,40 +150,40 @@ int32_t StepperMotor::getStepPhase() {
 
 // Returns the desired angle of the motor
 float StepperMotor::getDesiredAngle() {
-    #ifdef USE_HARDWARE_STEP_CNT
-        return (microstepAngle * getHardStepCNT());
-    #else
+    #ifdef USE_SOFTWARE_STEP_CNT
         return (microstepAngle * getSoftStepCNT());
+    #else
+        return (microstepAngle * getHardStepCNT());
     #endif
 }
 
 
 // Sets the desired angle of the motor
 void StepperMotor::setDesiredAngle(float newDesiredAngle) {
-    #ifdef USE_HARDWARE_STEP_CNT
-        setHardStepCNT(round(newDesiredAngle / microstepAngle));
-    #else
+    #ifdef USE_SOFTWARE_STEP_CNT
         setSoftStepCNT(round(newDesiredAngle / microstepAngle));
+    #else
+        setHardStepCNT(round(newDesiredAngle / microstepAngle));
     #endif
 }
 
 
 // Returns the desired step of the motor
 int32_t StepperMotor::getDesiredStep() {
-    #ifdef USE_HARDWARE_STEP_CNT
-        return getHardStepCNT();
-    #else
+    #ifdef USE_SOFTWARE_STEP_CNT
         return getSoftStepCNT();
+    #else
+        return getHardStepCNT();
     #endif
 }
 
 
 // Sets the desired step of the motor
 void StepperMotor::setDesiredStep(int32_t newDesiredStep) {
-    #ifdef USE_HARDWARE_STEP_CNT
-        setHardStepCNT(newDesiredStep);
-    #else
+    #ifdef USE_SOFTWARE_STEP_CNT
         setSoftStepCNT(newDesiredStep);
+    #else
+        setHardStepCNT(newDesiredStep);
     #endif
 }
 
@@ -223,9 +223,11 @@ void overflowHandler() {
         motor.stepOverflowOffset -= TIM_PERIOD;
     }
 }
-#else // ! USE_HARDWARE_STEP_CNT
+#endif // USE_HARDWARE_STEP_CNT
+
+#ifdef USE_SOFTWARE_STEP_CNT
 // Returns the desired step of the motor
-int32_t StepperMotor::getSoftStepCNT() {
+int32_t StepperMotor::getSoftStepCNT() const {
     return (this -> softStepCNT);
 }
 
@@ -234,7 +236,7 @@ int32_t StepperMotor::getSoftStepCNT() {
 void StepperMotor::setSoftStepCNT(int32_t newStepCNT) {
     this -> softStepCNT = newStepCNT;
 }
-#endif // ! USE_HARDWARE_STEP_CNT
+#endif // USE_SOFTWARE_STEP_CNT
 
 
 #ifdef ENABLE_DYNAMIC_CURRENT
@@ -347,10 +349,10 @@ void StepperMotor::setMicrostepping(uint8_t setMicrostepping, bool lock) {
         float stepScalingFactor = (setMicrostepping / this -> microstepDivisor);
 
         // Scale the step count
-        #ifdef USE_HARDWARE_STEP_CNT
-            setHardStepCNT(getHardStepCNT() * stepScalingFactor);
-        #else
+        #ifdef USE_SOFTWARE_STEP_CNT
             setSoftStepCNT(getSoftStepCNT() * stepScalingFactor);
+        #else
+            setHardStepCNT(getHardStepCNT() * stepScalingFactor);
         #endif
 
         // Scale the microstep multiplier so that the full stepping level is maintained
@@ -490,7 +492,7 @@ void StepperMotor::simpleStep() {
 
 // Computes the coil values for the next step position and increments the set angle
 #ifdef USE_HARDWARE_STEP_CNT
-void StepperMotor::step(STEP_DIR dir, int32_t stepChange) {
+void StepperMotor::step(STEP_DIR dir, int32_t stepChange, bool updateDesiredPos) {
 #else
 void StepperMotor::step(STEP_DIR dir, int32_t stepChange, bool updateDesiredPos) {
 #endif
@@ -540,7 +542,7 @@ void StepperMotor::step(STEP_DIR dir, int32_t stepChange, bool updateDesiredPos)
 
     stepChange *= dir * (this -> reversed);
 
-    #ifndef USE_HARDWARE_STEP_CNT
+    #ifdef USE_SOFTWARE_STEP_CNT
     // Update the desired angle if specified
     if (updateDesiredPos) {
 
diff --git a/src/hardware/motor.h b/src/hardware/motor.h
index 1e64c9e9..512a9a0a 100644
--- a/src/hardware/motor.h
+++ b/src/hardware/motor.h
@@ -105,7 +105,7 @@ class StepperMotor {
         void setDesiredStep(int32_t newDesiredStep);
 
         // Returns the desired step of the motor
-        int32_t getSoftStepCNT();
+        int32_t getSoftStepCNT() const;
 
         // Sets the desired step of the motor
         void setSoftStepCNT(int32_t newStepCNT);
@@ -195,7 +195,7 @@ class StepperMotor {
 
         // Calculates the coil values for the motor and updates the set angle.
         #ifdef USE_HARDWARE_STEP_CNT
-        void step(STEP_DIR dir, int32_t stepChange);
+        void step(STEP_DIR dir, int32_t stepChange, bool updateDesiredPos = true);
         #else
         void step(STEP_DIR dir, int32_t stepChange, bool updateDesiredPos = true);
         #endif
@@ -246,7 +246,7 @@ class StepperMotor {
         // Function that enables the motor
         void enable();
 
-        #ifndef USE_HARDWARE_STEP_CNT
+        #ifdef USE_SOFTWARE_STEP_CNT
             // Keeps the desired step of the motor
             int32_t softStepCNT = 0;
         #endif
diff --git a/src/hardware/oled.cpp b/src/hardware/oled.cpp
index 2ce52feb..ae901da9 100644
--- a/src/hardware/oled.cpp
+++ b/src/hardware/oled.cpp
@@ -228,6 +228,8 @@ void displayMotorData() {
 
     #endif // ! ENCODER_SPEED_ESTIMATION
 
+    snprintf(outBuffer, OB_SIZE, "sStp:% 10ld", motor.getSoftStepCNT());
+
     writeOLEDString(0, 0, outBuffer, false);
 
     // Angle error
@@ -240,6 +242,11 @@ void displayMotorData() {
 
     // Temp of the encoder (close to the motor temp)
     snprintf(outBuffer, OB_SIZE, "Temp:%8.1f C", motor.encoder.getTemp());
+
+    //snprintf(outBuffer, OB_SIZE, "dStp:% 10ld", motor.getDesiredStep());
+    //snprintf(outBuffer, OB_SIZE, "hStp:% 10ld", motor.getHardStepCNT());
+    snprintf(outBuffer, OB_SIZE, "h-s: % 10ld", motor.getHardStepCNT()-motor.getSoftStepCNT());
+
     writeOLEDString(0, LINE_HEIGHT * 3, outBuffer, true);
 }
 
diff --git a/src/hardware/timers.cpp b/src/hardware/timers.cpp
index 09764fb4..7d5bfe50 100644
--- a/src/hardware/timers.cpp
+++ b/src/hardware/timers.cpp
@@ -160,7 +160,7 @@ void disableInterrupts() {
 
     // Disable the interrupts if this is the first block
     if (interruptBlockCount == 0) {
-        __disable_irq();
+        //__disable_irq();
         syncInstructions();
     }
 
@@ -260,7 +260,8 @@ void stepMotor() {
 }
 
 
-// Need to declare a function to power the motor coils for the step interrupt
+#ifndef DISABLE_CORRECTION_TIMER
+    // Need to declare a function to power the motor coils for the step interrupt
 void correctMotor() {
     #ifdef CHECK_CORRECT_MOTOR_RATE
         GPIO_WRITE(LED_PIN, HIGH);
@@ -364,19 +365,19 @@ void correctMotor() {
 
                         // Motor is at a position smaller than the desired one
                         // Use the current angle to find the current step, then add 1
-                        #ifdef USE_HARDWARE_STEP_CNT
-                            motor.step(POSITIVE, 1);
-                        #else
+                        #ifdef USE_SOFTWARE_STEP_CNT
                             motor.step(POSITIVE, 1, false);
+                        #else
+                            motor.step(POSITIVE, 1);
                         #endif
                     }
                     else {
                         // Motor is at a position larger than the desired one
                         // Use the current angle to find the current step, then subtract 1
-                        #ifdef USE_HARDWARE_STEP_CNT
-                            motor.step(NEGATIVE, 1);
-                        #else
+                        #ifdef USE_SOFTWARE_STEP_CNT
                             motor.step(NEGATIVE, 1, false);
+                        #else
+                            motor.step(NEGATIVE, 1);
                         #endif
                     }
                 }
@@ -452,6 +453,7 @@ void correctMotor() {
         GPIO_WRITE(LED_PIN, LOW);
     #endif
 }
+#endif
 
 
 // Direct stepping
@@ -506,10 +508,10 @@ void stepScheduleHandler() {
     }
     else {
         // Just step the motor in the desired direction
-        #ifdef USE_HARDWARE_STEP_CNT
-            motor.step(scheduledStepDir, 1);
-        #else
+        #ifdef USE_SOFTWARE_STEP_CNT
             motor.step(scheduledStepDir, 1, false);
+        #else
+            motor.step(scheduledStepDir, 1);
         #endif
     }
 }
diff --git a/src/main/main.cpp b/src/main/main.cpp
index 3593c7c0..f50ac2a1 100644
--- a/src/main/main.cpp
+++ b/src/main/main.cpp
@@ -21,6 +21,8 @@ void setup() {
     // Set processor up
     SystemInit();
 
+    NVIC_SetPriorityGrouping(3); // __NVIC_PRIO_BITS
+
     // Configure the system clock
     #if defined(SYSCLK_SRC_HSE_16)
         #if SYSCLK_FREQ == 72