diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py
index 7ba4687..d983199 100644
--- a/remote_control/pid_controller.py
+++ b/remote_control/pid_controller.py
@@ -10,8 +10,8 @@ class PIDController:
         self.estimator = estimator
         self.controlling = False
 
-        self.P_angle = 0.5     # PID parameters determined by Ziegler-Nichols. measured K_crit = 1.4, T_crit = 1.5
-        self.I_angle = 0.4
+        self.P_angle = 0.4     # PID parameters determined by Ziegler-Nichols. measured K_crit = 1.4, T_crit = 1.5
+        self.I_angle = 0.35
         self.D_angle = 0.1
 
         self.P_pos = 0.50
@@ -206,51 +206,6 @@ class PIDController:
                     u2 = - u1
 
                     e_angle_old = e_angle
-                elif self.mode == 'position':
-                    # we assume that we face straight to the target position
-                    v = target_pos[0:2] - x_pred[0:2]
-                    # check if robot faces in same direction as pos-target-vector
-                    target_angle = math.atan2(v[1], v[0])
-                    current_angle = x_pred[2]
-
-                    angles_unwrapped = np.unwrap([current_angle, target_angle])  # unwrap angle to avoid jump in data
-                    alpha = angles_unwrapped[0]
-                    beta = angles_unwrapped[1]
-
-                    epsilon = 0.3
-
-
-                    if not (abs(alpha - beta) < epsilon or abs(alpha - beta + np.pi) < epsilon or abs(alpha - beta - np.pi) < epsilon):
-                        print("switch to angle mode")
-                        u1 = u2 = 0.0
-                        robot.send_cmd(u1, u2)
-                        self.mode = 'angle'
-                    elif abs(alpha - beta) < epsilon:
-                        # forward
-                        e = np.linalg.norm(x_pred[0:2] - target_pos[0:2])  # compute euclidean distance to target
-                        p = self.P * e
-                        # i += self.I * dt * e     # right Riemann sum
-                        i += self.I * dt * (e + e_old) / 2.0  # trapezoidal rule
-                        d = self.D * (e - e_old) / dt
-
-                        u1 = p - i + d
-                        u2 = u1
-                    else:
-                        # backward
-                        e = np.linalg.norm(x_pred[0:2] - target_pos[0:2])  # compute euclidean distance to target
-                        p = self.P * e
-                        # i += self.I * dt * e     # right Riemann sum
-                        i += self.I * dt * (e + e_old) / 2.0  # trapezoidal rule
-                        d = self.D * (e - e_old) / dt
-
-                        u1 = -(p - i + d)
-                        u2 = u1
-
-                    e_old = e
-                    #else:
-                    #    print("switching to angle mode")
-                    #    u1 = u2 = 0.0
-                    #    self.mode = 'angle'
 
                 elif self.mode == 'combined':
                     # compute angle such that robot faces to target point
@@ -262,22 +217,42 @@ class PIDController:
                     e_angle = angles_unwrapped[0] - angles_unwrapped[1]  # angle difference
                     e_pos = np.linalg.norm(v)
 
-                    p_angle = self.P_angle * e_angle
-                    i_angle += self.I_angle * dt * (e_angle + e_angle_old) / 2.0  # trapezoidal rule
-                    d_angle = self.D_angle * (e_angle - e_angle_old) / dt
-
-                    p_pos = self.P_pos * e_pos
-                    i_pos += self.I_pos * dt * (e_pos + e_pos_old) / 2.0  # trapezoidal rule
-                    d_pos = self.D_pos * (e_pos - e_pos_old) / dt
-
-                    u1 = p_angle + p_pos - i_angle - i_pos
-                    u2 = - p_angle + p_pos + i_angle - i_pos
-
-                    e_pos_old = e_pos
-                    e_angle_old = e_angle
-
                     if e_pos < 0.05:
                         self.mode = 'angle'
+                        e_angle_old = 0
+                        e_pos_old = 0
+                        i_angle = 0
+                        i_pos = 0
+                        u1 = 0
+                        u2 = 0
+                    else:
+                        forward = abs(e_angle) < np.pi/2.0
+
+                        if not forward:
+                            if e_angle > np.pi/2.0:
+                                e_angle -= np.pi
+                            else:
+                                e_angle += np.pi
+
+                        p_angle = self.P_angle * e_angle
+                        i_angle += self.I_angle * dt * (e_angle + e_angle_old) / 2.0  # trapezoidal rule
+                        d_angle = self.D_angle * (e_angle - e_angle_old) / dt
+
+                        p_pos = self.P_pos * e_pos
+                        i_pos += self.I_pos * dt * (e_pos + e_pos_old) / 2.0  # trapezoidal rule
+                        d_pos = self.D_pos * (e_pos - e_pos_old) / dt
+
+                        if forward:
+                            print("forward")
+                            u1 = p_angle + p_pos - i_angle - i_pos - d_angle - d_pos
+                            u2 = - p_angle + p_pos + i_angle - i_pos + d_angle - d_pos
+                        else:
+                            print("backward")
+                            u1 = p_angle - p_pos - i_angle + i_pos - d_angle + d_pos
+                            u2 = - p_angle - p_pos + i_angle + i_pos + d_angle + d_pos
+
+                        e_pos_old = e_pos
+                        e_angle_old = e_angle
 
                 else:
                     u1 = 0.0