diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py index b19397d..1caf473 100644 --- a/remote_control/position_controller.py +++ b/remote_control/position_controller.py @@ -124,7 +124,7 @@ class RemoteController: self.ols = OpenLoopSolver() self.ols.setup() - self.target = (0.0, 0.0) + 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) @@ -215,6 +215,18 @@ class RemoteController: 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: @@ -225,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: @@ -244,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: @@ -282,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 @@ -351,24 +404,44 @@ class RemoteController: if event.type == pygame.KEYDOWN: if event.key == pygame.K_UP: 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) + self.target = (0.0, 0.0, 0.0) elif event.key == pygame.K_1: - self.target = (0.5,0.5) + self.target = (0.5,0.5, -np.pi/2.0) elif event.key == pygame.K_2: - self.target = (0.5, -0.5) + self.target = (0.5, -0.5, 0.0) elif event.key == pygame.K_3: - self.target = (-0.5,-0.5) + self.target = (-0.5,-0.5, np.pi/2.0) elif event.key == pygame.K_4: - self.target = (-0.5,0.5) + self.target = (-0.5,0.5, 0.0) if self.controlling: tmpc_start = time.time() # get measurement - last_measurement = copy.deepcopy(self.xms[-1]) - res = self.ols.solve(last_measurement, self.target) + 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] @@ -396,16 +469,28 @@ class RemoteController: #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)) - for i in range(0, 5): + 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] - dt_mpc = time.time() - self.t - #if dt_mpc < 0.1: - # time.sleep(0.1 - dt_mpc) - self.rc_socket.send('({},{},{})\n'.format(0.1,u1, u2)) + self.rc_socket.send('({},{},{})\n'.format(dt,u1, u2)) self.t = time.time() - time.sleep(0.1) + #time.sleep(0.2) #