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

simple_control
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