From d170524370ccfd020d259b4fa245438d7f3d9c17 Mon Sep 17 00:00:00 2001 From: spirkelmann Date: Thu, 16 Jan 2020 10:58:33 +0100 Subject: [PATCH] added 8th waypoint --- remote_control/position_controller.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py index c76e206..3841e7e 100644 --- a/remote_control/position_controller.py +++ b/remote_control/position_controller.py @@ -367,6 +367,13 @@ class RemoteController: targets[j] = (mean[0], mean[1], 0.0) 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 current_target = 0 @@ -405,6 +412,8 @@ class RemoteController: self.target = targets[5] elif event.key == pygame.K_7: self.target = targets[6] + elif event.key == pygame.K_8: + self.target = targets[7] elif event.key == pygame.K_SPACE: auto_control = not auto_control if auto_control: @@ -437,7 +446,7 @@ class RemoteController: if auto_control: if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3: print("close to target!") - current_target = (current_target + 1) % 7 + current_target = (current_target + 1) % 8 self.target = targets[current_target] print("new target = {}".format(current_target))