Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

fixed a problem where robot could drive diagonally caused by int casting error and improved the socket buffer size which could lead to problems for longer messages

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-09-15 15:23:01 +02:00
parent e7ab92dfe2
commit 36f3ccbd6c
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
remote_control/roborally.py
View File

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