From 7962199c3a868788cade0d5df6d23b71f3f1559e Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Thu, 9 Sep 2021 21:36:13 +0200 Subject: [PATCH] reset time when stopping controller --- remote_control/pid_controller.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py index 6884472..54a26c5 100644 --- a/remote_control/pid_controller.py +++ b/remote_control/pid_controller.py @@ -70,9 +70,9 @@ class PIDController(ControllerBase): angles_unwrapped = np.unwrap([x_pred[2], target_angle]) # unwrap angle to avoid jump in data e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference - e_pos = np.linalg.norm(v) + e_pos = np.linalg.norm(v[0:2]) - if e_pos < 0.05: + if e_pos < 0.03: self.mode = 'angle' self.e_angle_old = 0 self.e_pos_old = 0 @@ -111,6 +111,10 @@ class PIDController(ControllerBase): u2 = 0.0 return u1, u2 + def stop(self): + super(PIDController, self).stop() + self.t = None + def apply_control(self, control): self.t = time.time() # save time when the most recent control was applied super(PIDController, self).apply_control(control)