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

added 8th waypoint

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-01-16 10:58:33 +01:00
parent 0c59a77b28
commit d170524370
1 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
remote_control/position_controller.py
View File

@ -367,6 +367,13 @@ class RemoteController:
targets[j] = (mean[0], mean[1], 0.0) targets[j] = (mean[0], mean[1], 0.0)
j += 1 j += 1
# final connection between first and last marker
mean_in = (np.array(markers_in[3]) + np.array(markers_in[0])) * 0.5
mean_out = (np.array(markers_out[3]) + np.array(markers_out[0])) * 0.5
mean = mean_in + (1.0 - lamb) * (mean_out - mean_in)
targets[j] = (mean[0], mean[1], 0.0)
print("targets = {}".format(targets.keys()))
auto_control = False auto_control = False
current_target = 0 current_target = 0
@ -405,6 +412,8 @@ class RemoteController:
self.target = targets[5] self.target = targets[5]
elif event.key == pygame.K_7: elif event.key == pygame.K_7:
self.target = targets[6] self.target = targets[6]
elif event.key == pygame.K_8:
self.target = targets[7]
elif event.key == pygame.K_SPACE: elif event.key == pygame.K_SPACE:
auto_control = not auto_control auto_control = not auto_control
if auto_control: if auto_control:
@ -437,7 +446,7 @@ class RemoteController:
if auto_control: if auto_control:
if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3: if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3:
print("close to target!") print("close to target!")
current_target = (current_target + 1) % 7 current_target = (current_target + 1) % 8
self.target = targets[current_target] self.target = targets[current_target]
print("new target = {}".format(current_target)) print("new target = {}".format(current_target))