diff --git a/remote_control/roborally.py b/remote_control/roborally.py index bcae352..9faa7bf 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -32,7 +32,7 @@ from collections import OrderedDict from argparse import ArgumentParser -MSGLEN = 32 +MSGLEN = 64 def myreceive(sock): chunks = [] bytes_recd = 0 @@ -111,8 +111,8 @@ class RemoteController: self.anim_stopped = False - #self.robots = [Robot(11, '192.168.1.101'), Robot(14, '192.168.1.103')] - self.robots = [Robot(11, '192.168.1.101')] + self.robots = [Robot(11, '192.168.1.11'), Robot(14, '192.168.1.14')] + #self.robots = [Robot(11, '192.168.1.101')] #self.robots = [Robot(15, '192.168.1.102')] #self.robots = [Robot(id, ip)] @@ -326,7 +326,7 @@ class RemoteController: while connected: try: data = myreceive(clientsocket) - print(data) + print("data received: ", data) inputs = data.split(b',') cmd = inputs[0] @@ -386,14 +386,16 @@ class RemoteController: robot = self.robot_ids[robot_id] # get robot to be controlled grid_pos = robot.grid_pos # grid position of the robot + print("old grid pos for robot {}: {}".format(robot_id, grid_pos)) + # compute new grid position and orientation if cmd == 'forward': - new_x = int(grid_pos[0] + 1 * np.cos(grid_pos[2])) - new_y = int(grid_pos[1] + 1 * np.sin(grid_pos[2])) + new_x = int(round(grid_pos[0] + 1 * np.cos(grid_pos[2]))) + new_y = int(round(grid_pos[1] + 1 * np.sin(grid_pos[2]))) new_angle = grid_pos[2] elif cmd == 'backward': - new_x = int(grid_pos[0] - 1 * np.cos(grid_pos[2])) - new_y = int(grid_pos[1] - 1 * np.sin(grid_pos[2])) + new_x = int(round(grid_pos[0] - 1 * np.cos(grid_pos[2]))) + new_y = int(round(grid_pos[1] - 1 * np.sin(grid_pos[2]))) new_angle = grid_pos[2] elif cmd == 'turn left': new_x = grid_pos[0] @@ -410,9 +412,11 @@ class RemoteController: else: print("unknown command!") sys.exit(1) + if new_x != grid_pos[0] and new_y != grid_pos[1]: + print("problem detected!") grid_pos = (new_x, new_y, new_angle) - print("new grid pos for robot {}: {}".format(robot_id, grid_pos)) + print("new grid pos for robot {}: {}\n".format(robot_id, grid_pos)) self.target = np.array((0.25 * grid_pos[0], 0.25 * grid_pos[1], grid_pos[2])) @@ -468,7 +472,7 @@ 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_pos = {}, error_ang = {}".format(error_pos, error_ang)) + #print("error_pos = {}, error_ang = {}".format(error_pos, error_ang)) if error_pos > 0.075 or error_ang > 0.35: # solve mpc open loop problem