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
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