diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py
index 6884472..54a26c5 100644
--- a/remote_control/pid_controller.py
+++ b/remote_control/pid_controller.py
@@ -70,9 +70,9 @@ class PIDController(ControllerBase):
             angles_unwrapped = np.unwrap([x_pred[2], target_angle])  # unwrap angle to avoid jump in data
 
             e_angle = angles_unwrapped[0] - angles_unwrapped[1]  # angle difference
-            e_pos = np.linalg.norm(v)
+            e_pos = np.linalg.norm(v[0:2])
 
-            if e_pos < 0.05:
+            if e_pos < 0.03:
                 self.mode = 'angle'
                 self.e_angle_old = 0
                 self.e_pos_old = 0
@@ -111,6 +111,10 @@ class PIDController(ControllerBase):
             u2 = 0.0
         return u1, u2
 
+    def stop(self):
+        super(PIDController, self).stop()
+        self.t =  None
+
     def apply_control(self, control):
         self.t = time.time()  # save time when the most recent control was applied
         super(PIDController, self).apply_control(control)