diff --git a/remote_control/roborally.py b/remote_control/roborally.py index def4c70..2fddcab 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -61,6 +61,26 @@ class Robot: self.euler = None self.ip = ip + self.socket = socket.socket() + + # variables for measurements + self.tms_0 = None + self.xm_0 = None + self.tms = None + self.xms = None + + # currently active control + self.u1 = 0.0 + self.u2 = 0.0 + + + def connect(self): + # connect to robot + try: + self.socket.connect((self.ip, 1234)) # connect to robot + except socket.error: + print("could not connect to socket") + sys.exit(1) def f_ode(t, x, u): # dynamical model of the two-wheeled robot @@ -89,9 +109,9 @@ class RemoteController: self.anim_stopped = False - #self.robots = [Robot(14, '192.168.1.103')] + self.robots = [Robot(11, '192.168.1.101'), Robot(14, '192.168.1.103')] #self.robots = [Robot(15, '192.168.1.102')] - self.robots = [Robot(id, ip)] + #self.robots = [Robot(id, ip)] self.robot_ids = {} for r in self.robots: @@ -99,20 +119,13 @@ class RemoteController: self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right'] - # connect to robot - self.rc_socket = socket.socket() - #self.rc_socket = None - try: - for r in self.robots: - self.rc_socket.connect((r.ip, 1234)) # connect to robot - except socket.error: - print("could not connect to socket") - sys.exit(1) - # socket for movement commands self.comm_socket = socket.socket() self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + for robot in self.robots: + robot.connect() + #self.comm_socket.bind((socket.gethostname(), 1337)) self.comm_socket.bind(('', 1337)) self.comm_socket.listen(5) @@ -124,11 +137,6 @@ class RemoteController: self.ts = np.array([]) self.xs = [] - # variables for measurements - self.tms_0 = None - self.xm_0 = None - self.tms = None - self.xms = None # variable for mpc open loop self.ol_x = None @@ -142,10 +150,6 @@ class RemoteController: # pid parameters self.controlling = False - # currently active control - self.u1 = 0.0 - self.u2 = 0.0 - # animation self.fig = plt.figure() self.ax = self.fig.add_subplot(2,2,1) @@ -187,7 +191,7 @@ class RemoteController: # integrator self.r = scipy.integrate.ode(f_ode) self.omega_max = 5.0 - self.control_scaling = 0.2 + self.control_scaling = 0.3 #self.omega_max = 13.32 @@ -289,21 +293,21 @@ class RemoteController: # save measured position and angle for plotting measurement = np.array([r.pos[0], r.pos[1], r.euler]) - if self.tms_0 is None: - self.tms_0 = time.time() - self.xm_0 = measurement + if r.tms_0 is None: + r.tms_0 = time.time() + r.xm_0 = measurement self.mutex.acquire() try: - self.tms = np.array([0.0]) - self.xms = measurement + r.tms = np.array([0.0]) + r.xms = measurement finally: self.mutex.release() else: self.mutex.acquire() try: - self.tms = np.vstack((self.tms, time.time() - self.tms_0)) - self.xms = np.vstack((self.xms, measurement)) + r.tms = np.vstack((r.tms, time.time() - r.tms_0)) + r.xms = np.vstack((r.xms, measurement)) finally: self.mutex.release() @@ -388,8 +392,8 @@ class RemoteController: self.t = time.time() elif event.key == pygame.K_DOWN: self.controlling = False - if self.rc_socket: - self.rc_socket.send('(0.0,0.0)\n') + if self.robot_ids[robot_id].socket: + self.robot_ids[robot_id].socket.send('(0.0,0.0)\n') elif event.key == pygame.K_0: self.target = np.array([0,0,0]) elif event.key == pygame.K_PLUS: @@ -403,20 +407,20 @@ class RemoteController: elif event.key == pygame.K_ESCAPE: print("quit!") self.controlling = False - if self.rc_socket: - self.rc_socket.send('(0.0,0.0)\n') + if self.robot_ids[robot_id].socket: + self.robot_ids[robot_id].socket.send('(0.0,0.0)\n') self.anim_stopped = True return elif event.key == pygame.QUIT: print("quit!") self.controlling = False - if self.rc_socket: - self.rc_socket.send('(0.0,0.0)\n') + if self.robot_ids[robot_id].socket: + self.robot_ids[robot_id].socket.send('(0.0,0.0)\n') self.anim_stopped = True return if self.mpc: - x_pred = self.get_measurement_prediction() + x_pred = self.get_measurement_prediction(robot_id) tmpc_start = time.time() @@ -424,9 +428,9 @@ class RemoteController: 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])) + print("error_pos = {}, error_ang = {}".format(error_pos, error_ang)) - if error_pos > 0.1 or error_ang > 0.35: + if error_pos > 0.075 or error_ang > 0.35: # solve mpc open loop problem res = self.ols.solve(x_pred, self.target) @@ -461,28 +465,28 @@ class RemoteController: for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1 u1 = us1[i] u2 = us2[i] - if self.rc_socket: - self.rc_socket.send('({},{})\n'.format(u1, u2)) + if self.robot_ids[robot_id].socket: + self.robot_ids[robot_id].socket.send('({},{})\n'.format(u1, u2)) if i < self.mstep: time.sleep(self.dt) self.t = time.time() # save time the most recent control was applied - def get_measurement_prediction(self): + def get_measurement_prediction(self, robot_id): # get measurement self.mutex.acquire() try: window = 3 - last_measurement = copy.deepcopy(self.xms[-window:]) + last_measurement = copy.deepcopy(self.robot_ids[robot_id].xms[-window:]) #print("last_measurements = {}".format(last_measurement)) #print("mean = {}".format(np.mean(last_measurement, axis=0))) last_measurement = np.mean(last_measurement, axis=0) - last_time = copy.deepcopy(self.tms[-1]) + last_time = copy.deepcopy(self.robot_ids[robot_id].tms[-1]) finally: self.mutex.release() # prediction of state at time the mpc will terminate - self.r.set_f_params(np.array([self.u1 * self.omega_max, self.u2 * self.omega_max])) + self.r.set_f_params(np.array([self.robot_ids[robot_id].u1 * self.omega_max, self.robot_ids[robot_id].u2 * self.omega_max])) self.r.set_initial_value(last_measurement, last_time)