added option to set target angle

micropython_firmware/main.py

@@ -4,8 +4,9 @@ from machine import I2C, Pin
 
 import d1motor
 
-import time
+import utime
 import usocket
+import uselect
 import esp
 
 class Robot:
@@ -32,6 +33,12 @@ class Robot:
         # setup socket for remote control 
         self.addr = usocket.getaddrinfo(ip, 1234)[0][-1]
 
+        self.poller = uselect.poll()
+        self.poller_timeout = 2 # timeout in ms
+
+        self.control_queue = []
+
+
     def remote_control(self):
         while True:
             print("setting up socket communication ...")
@@ -45,55 +52,124 @@ class Robot:
                     socket_setup_complete = True
                 except Exception as e:
                     print("could not create socket. error msg: {}\nwaiting 1 sec and retrying...".format(e))
-                    time.sleep(1.0)
+                    utime.sleep(1.0)
 
             print("waiting for connections on {} ...".format(self.addr))
             socket.listen(1)
             res = socket.accept() # this blocks until someone connects to the socket
             comm_socket = res[0]
+            self.poller.register(comm_socket, uselect.POLLIN)
             print("connected!")
             listening = True
+
+            duration_current = 0 
+            t_current = utime.ticks_ms()
+            duration_next = None
+            stopped = True
+            timeouts = 0 
             while listening:
-                 # expected data: '(u1, u2)'\n"
+                 elapsed = utime.ticks_ms() 
+                 remaining = duration_current - (elapsed-t_current)
+                 if remaining >= 0:
+                     timeouts = 0
+                     print("start of loop\n I have {} ms until next control needs to be applied".format(remaining))
+                 else:
+                     # current control timed out -> applying next control
+                     if len(self.control_queue) > 0:
+                         print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current))
+                         u_next = self.control_queue.pop(0)
+                         #print("duration of previous control = {}".format((elapsed - t_current)/1000.0))
+                         #print("applying new control (duration, u1, u2) = ({}, {}, {})".format(duration_next, u1_next, u2_next))
+                         # if so, apply it
+                         self.m1.speed(u_next[0])
+                         self.m2.speed(u_next[1])
+
+                         t_current = utime.ticks_ms()
+                         duration_current = duration_next
+
+                         stopped = False
+                     elif not stopped: 
+                         print("previous control applied for {} ms too long".format(elapsed - t_current - duration_current))
+                         #print("duration of previous control = {}".format((elapsed - t_current)/1000.0))
+                         # no new control available -> shutdown
+                         print("no new control available -> stopping")
+
+                         self.m1.speed(0)
+                         self.m2.speed(0)
+                         
+                         t_current = utime.ticks_ms()
+                         duration_current = 0 # as soon as new control will become available we directly want to apply it immediately
+
+                         stopped = True
+
+                 #elif timeouts < 10:
+                 #    print("start of loop\n I have {} ms until next control needs to be applied, timeouts = {}".format(remaining, timeouts))
+                 #    timeouts = timeouts + 1 
+                 
+                 trecv_start = utime.ticks_ms()
+
+                 # expected data: '(t, u1_0, u2_0, u1_1, u2_1, ...)'\n"
                  # where ui = control for motor i
                  # ui \in [-1.0, 1.0]
-                 try:
-                     data = comm_socket.readline()
-                     data_str = data.decode()
-                     #print("Data received: {}".format(data_str))
-                     #print("processing data = {}".format(data_str))
-                     l = data_str.strip('()\n').split(',')
-                     #print("l = {}".format(l))
-                     u1 = int(float(l[0])*100)
-                     #print("u1 = {}".format(u1))
-                     u2 = int(float(l[1])*100)
-                     #print("u2 = {}".format(u2))
-                 except ValueError:
-                     print("ValueError: Data has wrong format.")
-                     print("Data received: {}".format(data_str))
-                     print("Shutting down ...")
-                     u1 = u2 = 0
-                     listening = False
-                 except IndexError:
-                     print("IndexError: Data has wrong format.")
-                     print("Data received: {}".format(data_str))
-                     print("Shutting down ...")
-                     u1 = u2 = 0
-                     listening = False
-                 except Exception as e:
-                     print("Some other error occured")
-                     print("Exception: {}".format(e))
-                     print("Shutting down ...")
-                     u1 = u2 = 0
-                     listening = False
-                 finally:    
-                     self.m1.speed(u1)
-                     self.m2.speed(u2)
-            comm_socket.close()
-            socket.close()
-            del comm_socket
-            del socket
-            print("disconnected!")
+                 #print("poller waiting..")
+                 poll_res = self.poller.poll(self.poller_timeout) # wait 100 milliseconds for socket data
+                 if poll_res:
+                     print("new data available")
+                     try:
+                         data = comm_socket.readline()
+                         data_str = data.decode()
+                         #print("Data received: {}".format(data_str))
+                         #print("processing data = {}".format(data_str))
+                         l = data_str.strip('()\n').split(',')
+                         #print("l = {}".format(l))
+                         duration_next = int(float(l[0])*1000)
+                         #print("duration = {}".format(duration_next))
+                         self.control_queue = []
+                         print("putting data into queue")
+                         for i in range((len(l)-1)/2):
+                             u1_next = int(float(l[2*i+1])*100)
+                             print("u1 = {}".format(u1_next))
+                             u2_next = int(float(l[2*i+2])*100)
+                             print("u2 = {}".format(u2_next))
+                             self.control_queue.append((u1_next, u2_next))
+                     except ValueError:
+                         print("ValueError: Data has wrong format.")
+                         print("Data received: {}".format(data_str))
+                         print("Shutting down ...")
+                         self.control_queue = []
+                         duration_current = 0
+                         listening = False
+                         comm_socket.close()
+                         socket.close()
+                         del comm_socket
+                         del socket
+                         print("disconnected!")
+                     except IndexError:
+                         print("IndexError: Data has wrong format.")
+                         print("Data received: {}".format(data_str))
+                         print("Shutting down ...")
+                         self.control_queue = []
+                         duration_current = 0
+                         listening = False
+                         comm_socket.close()
+                         socket.close()
+                         del comm_socket
+                         del socket
+                         print("disconnected!")
+                     except Exception as e:
+                         print("Some other error occured")
+                         print("Exception: {}".format(e))
+                         print("Shutting down ...")
+                         self.control_queue = []
+                         duration_current = 0
+                         listening = False
+                         comm_socket.close()
+                         socket.close()
+                         del comm_socket
+                         del socket
+                         print("disconnected!")
+                     trecv_end = utime.ticks_ms()
+                     print("communication (incl. polling) took {} ms".format(trecv_end - trecv_start))
 
 wall_e = Robot()
 wall_e.remote_control()

remote_control/casadi_opt.py

@@ -1,13 +1,17 @@
 from casadi import *
+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=60, T=6.0):
+    def __init__(self, N=10, T=2.0):
         self.T = T
         self.N = N
 
-    def solve(self, x0):
+        self.opti_x0 = None
+        self.opti_lam_g0 = None
+
+    def setup(self):
         x = SX.sym('x')
         y = SX.sym('y')
         theta = SX.sym('theta')
@@ -15,29 +19,32 @@ class OpenLoopSolver:
         r = 0.03
         R = 0.05
         d = 0.02
+        #omega_max = 13.32
+        omega_max = 10.0
 
         omegar = SX.sym('omegar')
         omegal = SX.sym('omegal')
         control = vertcat(omegar, omegal)
 
         # model equation
-        f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar + (r / 2 * cos(theta) + r * d / (2 * R) * sin(
-            theta)) * omegal
-        f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar + (r / 2 * sin(theta) - r * d / (2 * R) * cos(
-            theta)) * omegal
-        f3 = r / (2 * R) * omegar - r / (2 * R) * omegal
+        f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar * omega_max + (r / 2 * cos(theta) + r * d / (2 * R) * sin(
+            theta)) * omegal * omega_max
+        f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar * omega_max + (r / 2 * sin(theta) - r * d / (2 * R) * cos(
+            theta)) * omegal * omega_max
+        f3 = -(r / (2 * R) * omegar - r / (2 * R) * omegal) * omega_max
         xdot = vertcat(f1, f2, f3)
         f = Function('f', [x, y, theta, omegar, omegal], [f1, f2, f3])
         print("f = {}".format(f))
 
         # cost functional
-        L = x ** 2 + y ** 2 + 1e-2 * theta ** 2 + 1e-4 * (omegar ** 2 + omegal ** 2)
+        target = (-0.0, 0.0)
+        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
         DT = self.T / self.N / M
         print("DT = {}".format(DT))
-        f = Function('f', [state, control], [xdot, L])
+        self.f = Function('f', [state, control], [xdot, L])
         X0 = MX.sym('X0', 3)
         U = MX.sym('U', 2)
         X = X0
@@ -45,10 +52,10 @@ class OpenLoopSolver:
         runge_kutta = True
         if runge_kutta:
             for j in range(M):
-                k1, k1_q = f(X, U)
-                k2, k2_q = f(X + DT / 2 * k1, U)
-                k3, k3_q = f(X + DT / 2 * k2, U)
-                k4, k4_q = f(X + DT * k3, U)
+                k1, k1_q = self.f(X, U)
+                k2, k2_q = self.f(X + DT / 2 * k1, U)
+                k3, k3_q = self.f(X + DT / 2 * k2, U)
+                k4, k4_q = self.f(X + DT * k3, U)
                 X = X + DT / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
                 Q = Q + DT / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
         else:
@@ -75,127 +82,185 @@ class OpenLoopSolver:
         ubg = []
 
         # Formulate the NLP
-        Xk = MX(x0)
-        for k in range(self.N):
-            # New NLP variable for the control
-            U1k = MX.sym('U1_' + str(k), 2)
-            # U2k = MX.sym('U2_' + str(k))
-            w += [U1k]
-            lbw += [-10, -10]
-            ubw += [10, 10]
-            w0 += [0, 0]
-
-            # Integrate till the end of the interval
-            Fk = F(x0=Xk, p=U1k)
-            Xk = Fk['xf']
-            J = J + Fk['qf']
-
-            # Add inequality constraint
-            # g += [Xk[1]]
-            # lbg += [-.0]
-            # ubg += [inf]
-
-        # Create an NLP solver
-        prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
-        self.solver = nlpsol('solver', 'ipopt', prob)
+        # Xk = MX(x0)
+        # for k in range(self.N):
+        #     # New NLP variable for the control
+        #     U1k = MX.sym('U1_' + str(k), 2)
+        #     # U2k = MX.sym('U2_' + str(k))
+        #     w += [U1k]
+        #     lbw += [-0.5, -0.5]
+        #     ubw += [0.5, 0.5]
+        #     w0 += [0, 0]
+        #
+        #     # Integrate till the end of the interval
+        #     Fk = F(x0=Xk, p=U1k)
+        #     Xk = Fk['xf']
+        #     J = J + Fk['qf']
+        #
+        #     # Add inequality constraint
+        #     # g += [Xk[1]]
+        #     # lbg += [-.0]
+        #     # ubg += [inf]
+        #
+        # # Create an NLP solver
+        # prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
+        # self.solver = nlpsol('solver', 'ipopt', prob)
 
         # Solve the NLP
-        sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
-        w_opt = sol['x']
+        if False:
+            sol = self.solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
+            w_opt = sol['x']
 
-        # Plot the solution
-        u_opt = w_opt
-        x_opt = [x0]
-        for k in range(self.N):
-            Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2])
-            x_opt += [Fk['xf'].full()]
-        x1_opt = [r[0] for r in x_opt]
-        x2_opt = [r[1] for r in x_opt]
-        x3_opt = [r[2] for r in x_opt]
+            # Plot the solution
+            u_opt = w_opt
+            x_opt = [self.x0]
+            for k in range(self.N):
+                Fk = F(x0=x_opt[-1], p=u_opt[2*k:2*k+2])
+                x_opt += [Fk['xf'].full()]
+            x1_opt = [r[0] for r in x_opt]
+            x2_opt = [r[1] for r in x_opt]
+            x3_opt = [r[2] for r in x_opt]
 
         tgrid = [self.T/self.N*k for k in range(self.N+1)]
-        import matplotlib.pyplot as plt
-        plt.figure(2)
-        plt.clf()
-        plt.plot(tgrid, x1_opt, '--')
-        plt.plot(tgrid, x2_opt, '-')
-        plt.plot(tgrid, x3_opt, '*')
+        #import matplotlib.pyplot as plt
+        #plt.figure(2)
+        #plt.clf()
+        #plt.plot(tgrid, x1_opt, '--')
+        #plt.plot(tgrid, x2_opt, '-')
+        #plt.plot(tgrid, x3_opt, '*')
         #plt.step(tgrid, vertcat(DM.nan(1), u_opt), '-.')
-        plt.xlabel('t')
-        plt.legend(['x1','x2','x3','u'])
-        plt.grid()
+        #plt.xlabel('t')
+        #plt.legend(['x1','x2','x3','u'])
+        #plt.grid()
         #plt.show()
         #return
 
         # alternative solution using multiple shooting (way faster!)
-        opti = Opti() # Optimization problem
+        self.opti = Opti() # Optimization problem
 
         # ---- decision variables ---------
-        X = opti.variable(3,self.N+1) # state trajectory
-        Q = opti.variable(1,self.N+1) # state trajectory
-        posx   = X[0,:]
-        posy   = X[1,:]
-        angle = X[2,:]
-        U = opti.variable(2,self.N)   # control trajectory (throttle)
-        #T = opti.variable()      # final time
+        self.X = self.opti.variable(3,self.N+1) # state trajectory
+        self.Q = self.opti.variable(1,self.N+1) # state trajectory
+
+        self.U = self.opti.variable(2,self.N)   # control trajectory (throttle)
+        #T = self.opti.variable()      # final time
 
         # ---- objective          ---------
-        #opti.minimize(T) # race in minimal time
+        #self.opti.minimize(T) # race in minimal time
 
         # ---- dynamic constraints --------
         #f = lambda x,u: vertcat(f1, f2, f3) # dx/dt = f(x,u)
 
-        dt = self.T/self.N # length of a control interval
-        for k in range(self.N): # loop over control intervals
-           # Runge-Kutta 4 integration
-           k1, k1_q = f(X[:,k],         U[:,k])
-           k2, k2_q = f(X[:,k]+dt/2*k1, U[:,k])
-           k3, k3_q = f(X[:,k]+dt/2*k2, U[:,k])
-           k4, k4_q = f(X[:,k]+dt*k3,   U[:,k])
-           x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4)
-           q_next = Q[:,k] + dt/6*(k1_q + 2 * k2_q + 2 * k3_q + k4_q)
-           opti.subject_to(X[:,k+1]==x_next) # close the gaps
-           opti.subject_to(Q[:,k+1]==q_next) # close the gaps
-        opti.minimize(Q[:,self.N])
-
-        # ---- path constraints -----------
-        #limit = lambda pos: 1-sin(2*pi*pos)/2
-        #opti.subject_to(speed<=limit(pos))   # track speed limit
-        opti.subject_to(opti.bounded(-10,U,10)) # control is limited
-
-        # ---- boundary conditions --------
-        opti.subject_to(posx[0]==x0[0])   # start at position 0 ...
-        opti.subject_to(posy[0]==x0[1]) # ... from stand-still
-        opti.subject_to(angle[0]==x0[2])  # finish line at position 1
-        #opti.subject_to(speed[-1]==0)  # .. with speed 0
-        opti.subject_to(Q[:,0]==0.0)
-
-        # ---- misc. constraints  ----------
-        #opti.subject_to(X[1,:]>=0) # Time must be positive
-        #opti.subject_to(X[2,:]<=4) # Time must be positive
-        #opti.subject_to(X[2,:]>=-2) # Time must be positive
-
-        # avoid obstacle
-        #r = 0.25
-        #p = (0.5, 0.5)
-        #for k in range(self.N):
-        #    opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2  > r**2)
-        #    pass
 
 
         # ---- initial values for solver ---
-        #opti.set_initial(speed, 1)
-        #opti.set_initial(T, 1)
+        #self.opti.set_initial(speed, 1)
+        #self.opti.set_initial(T, 1)
 
+
+    def solve(self, x0, target):
+
+        tstart = time.time()
+        x = SX.sym('x')
+        y = SX.sym('y')
+        theta = SX.sym('theta')
+        state = vertcat(x, y, theta)
+        r = 0.03
+        R = 0.05
+        d = 0.02
+        omega_max = 13.32
+
+        omegar = SX.sym('omegar')
+        omegal = SX.sym('omegal')
+        control = vertcat(omegar, omegal)
+
+        # model equation
+        f1 = (r / 2 * cos(theta) - r * d / (2 * R) * sin(theta)) * omegar * omega_max + (r / 2 * cos(theta) + r * d / (
+        2 * R) * sin(
+            theta)) * omegal * omega_max
+        f2 = (r / 2 * sin(theta) + r * d / (2 * R) * cos(theta)) * omegar * omega_max + (r / 2 * sin(theta) - r * d / (
+        2 * R) * cos(
+            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)
+        self.f = Function('f', [state, control], [xdot, L])
         # ---- solve NLP              ------
-        opti.solver("ipopt") # set numerical backend
-        sol = opti.solve()   # actual solve
 
-        #x0 = sol.value(opti.x)
-        #lam_g0 = sol.value(opti.lam_g)
-        #opti.set_initial(opti.lam_g, lam_g0)
-        #opti.set_initial(opti.x, x0)
-        #opti.solve()
+         # set numerical backend
+
+
+        # delete constraints
+        self.opti.subject_to()
+
+        # add new constraints
+        dt = self.T / self.N  # length of a control interval
+        for k in range(self.N):  # loop over control intervals
+            # Runge-Kutta 4 integration
+            k1, k1_q = self.f(self.X[:, k], self.U[:, k])
+            k2, k2_q = self.f(self.X[:, k] + dt / 2 * k1, self.U[:, k])
+            k3, k3_q = self.f(self.X[:, k] + dt / 2 * k2, self.U[:, k])
+            k4, k4_q = self.f(self.X[:, k] + dt * k3, self.U[:, k])
+            x_next = self.X[:, k] + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
+            q_next = self.Q[:, k] + dt / 6 * (k1_q + 2 * k2_q + 2 * k3_q + k4_q)
+            self.opti.subject_to(self.X[:, k + 1] == x_next)  # close the gaps
+            self.opti.subject_to(self.Q[:, k + 1] == q_next)  # close the gaps
+        self.opti.minimize(self.Q[:, self.N])
+
+        # ---- path constraints -----------
+        # limit = lambda pos: 1-sin(2*pi*pos)/2
+        # self.opti.subject_to(speed<=limit(pos))   # track speed limit
+        maxcontrol = 0.5
+        self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol))  # control is limited
+
+        # ---- boundary conditions --------
+
+        # self.opti.subject_to(speed[-1]==0)  # .. with speed 0
+        self.opti.subject_to(self.Q[:, 0] == 0.0)
+
+
+        solver = self.opti.solver("ipopt", {}, {"print_level": 0})
+
+        # ---- misc. constraints  ----------
+        # self.opti.subject_to(X[1,:]>=0) # Time must be positive
+        # self.opti.subject_to(X[2,:]<=4) # Time must be positive
+        # self.opti.subject_to(X[2,:]>=-2) # Time must be positive
+
+        # avoid obstacle
+        # r = 0.25
+        # p = (0.5, 0.5)
+        # for k in range(self.N):
+        #    self.opti.subject_to((X[0,k]-p[0])**2 + (X[1,k]-p[1])**2  > r**2)
+        #    pass
+        posx = self.X[0, :]
+        posy = self.X[1, :]
+        angle = self.X[2, :]
+
+        self.opti.subject_to(posx[0] == x0[0])  # start at position 0 ...
+        self.opti.subject_to(posy[0] == x0[1])  # ... from stand-still
+        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))
+
+        if self.opti_x0 is not None:
+            self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
+            self.opti.set_initial(self.opti.x, self.opti_x0)
+        sol = self.opti.solve()   # actual solve
+
+        self.opti_x0 = sol.value(self.opti.x)
+        self.opti_lam_g0 = sol.value(self.opti.lam_g)
+
+        #u_opt_1 = map(lambda x: float(x), [u_opt[i * 2] for i in range(0, 60)])
+        #u_opt_2 = map(lambda x: float(x), [u_opt[i * 2 + 1] for i in range(0, 60)])
+        u_opt_1 = sol.value(self.U[0,:])
+        u_opt_2 = sol.value(self.U[1,:])
+
+        return (u_opt_1, u_opt_2)
+
+        #lam_g0 = sol.value(self.opti.lam_g)
+
+        #self.opti.solve()
 
         from pylab import plot, step, figure, legend, show, spy
 
@@ -206,8 +271,8 @@ class OpenLoopSolver:
         plt.figure(3)
         plot(sol.value(posx), sol.value(posy))
         ax = plt.gca()
-        circle = plt.Circle(p, r)
-        ax.add_artist(circle)
+        #circle = plt.Circle(p, r)
+        #ax.add_artist(circle)
         #plot(limit(sol.value(pos)),'r--',label="speed limit")
         #step(range(N),sol.value(U),'k',label="throttle")
         legend(loc="upper left")

remote_control/position_controller.py

@@ -11,6 +11,7 @@ import pygame
 import numpy as np
 import socket
 import scipy.integrate
+import copy
 
 import threading
 from copy import deepcopy
@@ -118,8 +119,12 @@ class RemoteController:
         self.dirs, = self.ax.plot([], [])
         plt.xlabel('x-position')
         plt.ylabel('y-position')
+        plt.grid()
 
         self.ols = OpenLoopSolver()
+        self.ols.setup()
+
+        self.target = (0.0, 0.0, 0.0)
 
     def ani(self):
         self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
@@ -207,6 +212,21 @@ class RemoteController:
                     self.mutex.release()
 
     def controller(self):
+        tgrid = None
+        us1 = None
+        us2 = None
+        u1 = -0.0
+        u2 = 0.0
+
+        r = scipy.integrate.ode(f_ode)
+
+        omega_max = 5.0
+
+        init_pos = None
+        init_time = None
+        final_pos = None
+        final_time = None
+        forward = True
         print("starting control")
         while True:
 
@@ -217,7 +237,7 @@ class RemoteController:
 
             if keyboard_control: # keyboard controller
                 events = pygame.event.get()
-                speed = 0.5
+                speed = 1.0
                 for event in events:
                     if event.type == pygame.KEYDOWN:
                         if event.key == pygame.K_LEFT:
@@ -236,17 +256,17 @@ class RemoteController:
                             self.u1 = -speed
                             self.u2 = -speed
                             #print("forward: ({},{})".format(u1, u2))
-                        self.rc_socket.send('({},{})\n'.format(self.u1, self.u2))
+                        self.rc_socket.send('({},{},{})\n'.format(0.1, self.u1, self.u2))
                     elif event.type == pygame.KEYUP:
                         self.u1 = 0
                         self.u2 = 0
                         #print("key released, resetting: ({},{})".format(u1, u2))
-                        self.rc_socket.send('({},{})\n'.format(self.u1, self.u2))
+                        self.rc_socket.send('({}, {},{})\n'.format(0.1, self.u1, self.u2))
 
                 tnew = time.time()
                 dt = tnew - self.t
                 r = scipy.integrate.ode(f_ode)
-                r.set_f_params(np.array([self.u1 * 13.32, self.u2 * 13.32]))
+                r.set_f_params(np.array([self.u1 * omega_max, self.u2 * omega_max]))
 
                 #print(self.x0)
                 if self.x0 is None:
@@ -274,18 +294,59 @@ class RemoteController:
                 events = pygame.event.get()
                 for event in events:
                     if event.type == pygame.KEYDOWN:
-                        if event.key == pygame.K_LEFT:
-                            self.speed = self.speed / np.sqrt(np.sqrt(np.sqrt(10.0)))
-                        elif event.key == pygame.K_RIGHT:
-                            self.speed = self.speed * np.sqrt(np.sqrt(np.sqrt(10.0)))
-                        elif event.key == pygame.K_UP:
-                            u1 = self.speed
-                            u2 = -self.speed
-                        elif event.key == pygame.K_DOWN:
-                            u1 = 0.0
-                            u2 = 0.0
-                        print("speed = {}".format(self.speed))
-                        self.rc_socket.send('({},{})\n'.format(u1, u2))
+                        if event.key == pygame.K_1:
+                            self.controlling = True
+                            forward = True
+                            print("starting test")
+                            self.mutex.acquire()
+                            try:
+                                init_pos = copy.deepcopy(self.xms[-1])
+                                init_time = copy.deepcopy(self.tms[-1])
+                            finally:
+                                self.mutex.release()
+                        if event.key == pygame.K_2:
+                            self.controlling = True
+                            forward = False
+                            print("starting test")
+                            self.mutex.acquire()
+                            try:
+                                init_pos = copy.deepcopy(self.xms[-1])
+                                init_time = copy.deepcopy(self.tms[-1])
+                            finally:
+                                self.mutex.release()
+                        elif event.key == pygame.K_3:
+                            self.controlling = False
+                            print("stopping test")
+                            self.rc_socket.send('(0.1, 0.0,0.0)\n')
+
+                            self.mutex.acquire()
+                            try:
+                                final_pos = copy.deepcopy(self.xms[-1])
+                                final_time = copy.deepcopy(self.tms[-1])
+                            finally:
+                                self.mutex.release()
+
+                            print("init_pos = {}".format(init_pos))
+                            print("final_pos = {}".format(final_pos))
+                            print("distance = {}".format(np.linalg.norm(init_pos[0:2]-final_pos[0:2])))
+                            print("dt = {}".format(final_time - init_time))
+
+                            d = np.linalg.norm(init_pos[0:2]-final_pos[0:2])
+                            t = final_time - init_time
+                            r = 0.03
+
+                            angular_velocity = d/r/t
+                            print("average angular velocity = {}".format(angular_velocity))
+
+
+                if self.controlling:
+                    if forward:
+                        self.rc_socket.send('(0.1, 1.0,1.0)\n')
+                    else:
+                        self.rc_socket.send('(0.1, -1.0,-1.0)\n')
+                    time.sleep(0.1)
+                    #print("speed = {}".format(self.speed))
+
 
             elif pid:
                 # pid controller
@@ -342,7 +403,98 @@ class RemoteController:
                 for event in events:
                     if event.type == pygame.KEYDOWN:
                         if event.key == pygame.K_UP:
-                            self.ols.solve(self.xms[-1])
+                            self.controlling = True
+                            self.t = time.time()
+                        elif event.key == pygame.K_DOWN:
+                            self.controlling = False
+                            self.rc_socket.send('(0.1, 0.0,0.0)\n')
+                        elif event.key == pygame.K_0:
+                            self.target = (0.0, 0.0, 0.0)
+                        elif event.key == pygame.K_1:
+                            self.target = (0.5,0.5, -np.pi/2.0)
+                        elif event.key == pygame.K_2:
+                            self.target = (0.5, -0.5, 0.0)
+                        elif event.key == pygame.K_3:
+                            self.target = (-0.5,-0.5, np.pi/2.0)
+                        elif event.key == pygame.K_4:
+                            self.target = (-0.5,0.5, 0.0)
+                if self.controlling:
+                    tmpc_start = time.time()
+                    # get measurement
+                    self.mutex.acquire()
+                    try:
+                        last_measurement = copy.deepcopy(self.xms[-1])
+                        last_time = copy.deepcopy(self.tms[-1])
+                    finally:
+                        self.mutex.release()
+
+                    print("current measurement (t, x) = ({}, {})".format(last_time, last_measurement))
+                    print("current control (u1, u2) = ({}, {})".format(u1, u2))
+
+                    # prediction of state at time the mpc will terminate
+                    r.set_f_params(np.array([u1 * omega_max, u2 * omega_max]))
+
+                    r.set_initial_value(last_measurement, last_time)
+                    dt = self.ols.T/self.ols.N
+                    print("integrating for {} seconds".format((dt)))
+                    x_pred = r.integrate(r.t + (dt))
+
+                    print("predicted initial state x_pred = ({})".format(x_pred))
+
+                    res = self.ols.solve(x_pred, self.target)
+                    #tgrid = res[0]
+                    us1 = res[0]
+                    us2 = res[1]
+
+                    # tt = 0
+                    # x = last_measurement
+                    # t_ol = np.array([tt])
+                    # x_ol = np.array([x])
+                    # # compute open loop prediction
+                    # for i in range(len(us1)):
+                    #     r = scipy.integrate.ode(f_ode)
+                    #     r.set_f_params(np.array([us1[i] * 13.32, us2[i] * 13.32]))
+                    #     r.set_initial_value(x, tt)
+                    #
+                    #     tt = tt + 0.1
+                    #     x = r.integrate(tt)
+                    #
+                    #     t_ol = np.vstack((t_ol, tt))
+                    #     x_ol = np.vstack((x_ol, x))
+
+                    #plt.figure(4)
+                    #plt.plot(x_ol[:,0], x_ol[:,1])
+
+
+                #if event.key == pygame.K_DOWN:
+                #    if tgrid is not None:
+                    tmpc_end = time.time()
+                    print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
+                    dt_mpc = time.time() - self.t
+                    if dt_mpc < dt:
+                        print("sleeping for {} seconds...".format(dt - dt_mpc))
+                        time.sleep(dt - dt_mpc)
+
+                    self.mutex.acquire()
+                    try:
+                        second_measurement = copy.deepcopy(self.xms[-1])
+                        second_time = copy.deepcopy(self.tms[-1])
+                    finally:
+                        self.mutex.release()
+
+                    print("(last_time, second_time, dt) = ({}, {}, {})".format(last_time, second_time, second_time - last_time))
+                    print("mismatch between predicted state and measured state: {}\n\n".format(second_measurement - last_measurement))
+
+                    for i in range(0, 1):
+                        u1 = us1[i]
+                        u2 = us2[i]
+                        self.rc_socket.send('({},{},{})\n'.format(dt,u1, u2))
+                        self.t = time.time()
+                        #time.sleep(0.2)
+                    #
+
+
+                    pass
 
 def main(args):
     rospy.init_node('controller_node', anonymous=True)