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

reset time when stopping controller

master
Simon Pirkelmann 2021-09-09 21:36:13 +02:00
parent f3babdcf0a
commit 7962199c3a
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
remote_control/pid_controller.py
View File

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