diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py
index 87df398..cf13a3c 100644
--- a/remote_control/casadi_opt.py
+++ b/remote_control/casadi_opt.py
@@ -3,6 +3,9 @@ import time
 
 # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
 
+# TODO for roborally
+# implement essential movements: forward, backward, turn left, turn right on grid-based level
+
 class OpenLoopSolver:
     def __init__(self, N=20, T=4.0):
         self.T = T
@@ -40,7 +43,8 @@ class OpenLoopSolver:
 
         # cost functional
         target = (-0.0, 0.0)
-        L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
+        L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
+        #L = (x-target[0]) ** 2 + (y-target[1]) ** 2 + 1e-2 * theta ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
 
         # Fixed step Runge-Kutta 4 integrator
         M = 4  # RK4 steps per interval
@@ -138,7 +142,12 @@ class OpenLoopSolver:
         #return
 
 
-    def solve(self, x0, target, obstacles, track):
+    def solve(self, x0, target, obstacles, turn=False):
+
+        angles_unwrapped = np.unwrap([x0[2], target[2]])  # unwrap angle to avoid jump in data
+        x0[2] = angles_unwrapped[0]
+        target[2] = angles_unwrapped[1]
+
         tstart = time.time()
         # alternative solution using multiple shooting (way faster!)
         self.opti = Opti() # Optimization problem
@@ -186,7 +195,11 @@ class OpenLoopSolver:
             theta)) * omegal * omega_max
         f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max
         xdot = vertcat(f1, f2, f3)
-        L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * (theta - target[2]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
+
+        #L = 100 * (x - target[0]) ** 2 + 100 * (y - target[1]) ** 2  + 1e-1 * (theta - target[2])**2 + 1e-2 * (omegar ** 2 + omegal ** 2)
+        L = 100 * (x - target[0]) ** 2 + 100 * (y - target[1]) ** 2 + 1e-1 * (target[2] - theta) ** 2 + 1e-2 * (
+                    omegar ** 2 + omegal ** 2)
+        #L = (x - target[0]) ** 2 + (y - target[1]) ** 2 + 1e-2 * (theta - target[2]) ** 2 + 1e-2 * (omegar ** 2 + omegal ** 2)
         self.f = Function('f', [state, control], [xdot, L])
         # ---- solve NLP              ------
 
@@ -222,7 +235,7 @@ class OpenLoopSolver:
         self.opti.subject_to(self.Q[:, 0] == 0.0)
 
 
-        solver = self.opti.solver("ipopt", {}, {"print_level": 0})
+        solver = self.opti.solver("ipopt", {'print_time': False}, {"print_level": 0})
 
         # ---- misc. constraints  ----------
         # self.opti.subject_to(X[1,:]>=0) # Time must be positive
@@ -256,7 +269,7 @@ class OpenLoopSolver:
         self.opti.subject_to(angle[0] == x0[2])  # finish line at position 1
         tend = time.time()
 
-        print("setting up  problem took {} seconds".format(tend - tstart))
+        #print("setting up  problem took {} seconds".format(tend - tstart))
 
         tstart = time.time()
         if self.use_warmstart and self.opti_x0 is not None:
@@ -267,7 +280,7 @@ class OpenLoopSolver:
                 print("could not set warmstart")
         sol = self.opti.solve()   # actual solve
         tend = time.time()
-        print("solving the problem took {} seconds".format(tend - tstart))
+        #print("solving the problem took {} seconds".format(tend - tstart))
 
         tstart = time.time()
         self.opti_x0 = sol.value(self.opti.x)
@@ -278,7 +291,7 @@ class OpenLoopSolver:
         u_opt_1 = sol.value(self.U[0,:])
         u_opt_2 = sol.value(self.U[1,:])
         tend = time.time()
-        print("postprocessing took {} seconds".format(tend - tstart))
+        #print("postprocessing took {} seconds".format(tend - tstart))
 
         return (u_opt_1, u_opt_2, sol.value(posx), sol.value(posy))
 
diff --git a/remote_control/roborally.py b/remote_control/roborally.py
index 6a7f1af..15d32f5 100644
--- a/remote_control/roborally.py
+++ b/remote_control/roborally.py
@@ -408,7 +408,6 @@ class RemoteController:
                     elif event.key == pygame.K_w:
                         move = 1
                         turn = 0
-
                     elif event.key == pygame.K_s:
                         move = -1
                         turn = 0
@@ -419,7 +418,8 @@ class RemoteController:
                     elif event.key == pygame.K_d:
                         turn = -1
                         move = 0
-                        integ = 0
+                    elif event.key == pygame.K_r:
+                        turn = 2
                     elif event.key == pygame.K_p:
                         self.pid = True
                     elif event.key == pygame.K_SPACE:
@@ -459,7 +459,7 @@ class RemoteController:
                     elif turn != 0:
                         new_x = grid_pos[0]
                         new_y = grid_pos[1]
-                        new_angle = grid_pos[2] + turn * np.pi/2
+                        new_angle = np.unwrap([0, grid_pos[2] + turn * np.pi/2])[1]
                         grid_pos = (new_x, new_y, new_angle)
 
                         print(grid_pos)
@@ -473,15 +473,16 @@ class RemoteController:
                     tmpc_start = time.time()
 
                     error_pos = np.linalg.norm(x_pred[0:2] - self.target[0:2])
-                    error_ang = np.abs(x_pred[2] - self.target[2])
-                    print("error pos = ", error_pos, " error_ang = ", error_ang)
+                    angles_unwrapped = np.unwrap([x_pred[2], self.target[2]])   # unwrap angle to avoid jump in data
+                    error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
+                    #print("error pos = ", error_pos)
+                    print(" error_ang = {}, target = {}, angle = {}".format(error_ang, self.target[2], x_pred[2]))
 
                     turning = turn != 0
                     if error_pos > 0.1 or error_ang > 0.4:
                         # solve mpc open loop problem
                         res = self.ols.solve(x_pred, self.target, [], turning)
 
-
                         #us1 = res[0]
                         #us2 = res[1]
                         us1 = res[0] * control_scaling