diff --git a/Mechaduino/Mechaduino/Controller.cpp b/Mechaduino/Mechaduino/Controller.cpp
index f88ac02..366f784 100644
--- a/Mechaduino/Mechaduino/Controller.cpp
+++ b/Mechaduino/Mechaduino/Controller.cpp
@@ -67,16 +67,25 @@ void TC5_Handler() {                // gets called with FPID frequency
 
     y_1 = y;  //copy current value of y to previous value (y_1) for next control cycle before PA angle added
 
+    // Depending on direction we want to apply torque, add or subtract a phase angle of PA for max effective torque.
+    // PA should be equal to one full step angle: if the excitation angle is the same as the current position, we would not move!
+    // If we are close to target (u is close to 0), phase angle is adjusted to intermediate value to reduce oscillations.
+    float pa = PA;
+
+    if (u > -deadband && u < deadband)
+    {
+      pa = PA * abs(u) / deadband;
+    }
     
-    if (u > 0)          //Depending on direction we want to apply torque, add or subtract a phase angle of PA for max effective torque.  PA should be equal to one full step angle: if the excitation angle is the same as the current position, we would not move!  
+    if (u > 0)
       {                 //You can experiment with "Phase Advance" by increasing PA when operating at high speeds
-      y += PA;          //update phase excitation angle
+      y += pa;          //update phase excitation angle
       if (u > uMAX)     // limit control effort
         u = uMAX;       //saturation limits max current command
       }
     else
       {
-      y -= PA;          //update phase excitation angle
+      y -= pa;          //update phase excitation angle
       if (u < -uMAX)    // limit control effort
         u = -uMAX;      //saturation limits max current command
       }
diff --git a/Mechaduino/Mechaduino/Parameters.cpp b/Mechaduino/Mechaduino/Parameters.cpp
index cd4666f..1436493 100644
--- a/Mechaduino/Mechaduino/Parameters.cpp
+++ b/Mechaduino/Mechaduino/Parameters.cpp
@@ -44,6 +44,8 @@ const float iMAX = 1.0;             // Be careful adjusting this.  While the A49
 const float rSense = 0.150;
 volatile int uMAX = (255/3.3)*(iMAX*10*rSense);   // 255 for 8-bit pwm, 1023 for 10 bit, must also edit analogFastWrite
 
+volatile int deadband = uMAX / 16;
+
 // A sine lookup table is faster than using the built in sin() function
 // for motor commutation... shifted by 0 degrees (this appears to allow
 // for a less noisy cal routine) and multiplied by 1024
diff --git a/Mechaduino/Mechaduino/Parameters.h b/Mechaduino/Mechaduino/Parameters.h
index e38e065..8f7178f 100644
--- a/Mechaduino/Mechaduino/Parameters.h
+++ b/Mechaduino/Mechaduino/Parameters.h
@@ -36,11 +36,12 @@ extern const float aps; // angle per step
 extern int cpr; //counts per rev
 extern const float stepangle;
 
-extern volatile float PA;  //
+extern volatile float PA;  // Phase advance
 
 extern const float iMAX;
 extern const float rSense;
 extern volatile int uMAX;
+extern volatile int deadband; // Deadband for PID loop, in this range phase is adjusted between -PA and +PA
 
 
 extern const int sin_1[];
diff --git a/Mechaduino/Mechaduino/Utils.cpp b/Mechaduino/Mechaduino/Utils.cpp
index e583020..400e652 100644
--- a/Mechaduino/Mechaduino/Utils.cpp
+++ b/Mechaduino/Mechaduino/Utils.cpp
@@ -693,10 +693,19 @@ void print_angle()                ///////////////////////////////////       PRIN
   SerialUSB.print(" , ");
 //  SerialUSB.print(stepNumber * aps, DEC);
 //  SerialUSB.print(" , ");
-  SerialUSB.print("Angle: ");
-  SerialUSB.print(read_angle(), 2);
-  SerialUSB.print(", raw encoder: ");
-  SerialUSB.print(readEncoder());
+  if (TC5->COUNT16.CTRLA.reg & TC_CTRLA_ENABLE)
+  {
+    // Print realtime data when in closed loop mode
+    SerialUSB.print("Angle: ");
+    SerialUSB.print(y_1, 2);
+  }
+  else
+  {
+    SerialUSB.print("Angle: ");
+    SerialUSB.print(read_angle(), 2);
+    SerialUSB.print(", raw encoder: ");
+    SerialUSB.print(readEncoder());
+  }
   SerialUSB.println();
 }