diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py
index 9acb362..4ff5abf 100644
--- a/remote_control/casadi_opt.py
+++ b/remote_control/casadi_opt.py
@@ -4,7 +4,7 @@ import time
 # look at: https://github.com/casadi/casadi/blob/master/docs/examples/python/vdp_indirect_multiple_shooting.py
 
 class OpenLoopSolver:
-    def __init__(self, N=20, T=2.0):
+    def __init__(self, N=10, T=2.0):
         self.T = T
         self.N = N
 
@@ -19,7 +19,8 @@ class OpenLoopSolver:
         r = 0.03
         R = 0.05
         d = 0.02
-        omega_max = 13.32
+        #omega_max = 13.32
+        omega_max = 10.0
 
         omegar = SX.sym('omegar')
         omegal = SX.sym('omegal')
@@ -182,7 +183,7 @@ 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 ** 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              ------
 
@@ -209,7 +210,8 @@ class OpenLoopSolver:
         # ---- path constraints -----------
         # limit = lambda pos: 1-sin(2*pi*pos)/2
         # self.opti.subject_to(speed<=limit(pos))   # track speed limit
-        self.opti.subject_to(self.opti.bounded(-0.5, self.U, 0.5))  # control is limited
+        maxcontrol = 0.5
+        self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol))  # control is limited
 
         # ---- boundary conditions --------