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

enabled backwards driving and adjusted parameters a bit

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-11-09 22:07:09 +01:00
parent 91cde26908
commit a872232880
1 changed files with 36 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
remote_control/pid_controller.py
View File

@ -10,8 +10,8 @@ class PIDController:
self.estimator = estimator
self.controlling = False
self.P_angle = 0.5 # PID parameters determined by Ziegler-Nichols. measured K_crit = 1.4, T_crit = 1.5
self.I_angle = 0.4
self.P_angle = 0.4 # PID parameters determined by Ziegler-Nichols. measured K_crit = 1.4, T_crit = 1.5
self.I_angle = 0.35
self.D_angle = 0.1
self.P_pos = 0.50
@ -206,51 +206,6 @@ class PIDController:
u2 = - u1
e_angle_old = e_angle
elif self.mode == 'position':
# we assume that we face straight to the target position
v = target_pos[0:2] - x_pred[0:2]
# check if robot faces in same direction as pos-target-vector
target_angle = math.atan2(v[1], v[0])
current_angle = x_pred[2]
angles_unwrapped = np.unwrap([current_angle, target_angle]) # unwrap angle to avoid jump in data
alpha = angles_unwrapped[0]
beta = angles_unwrapped[1]
epsilon = 0.3
if not (abs(alpha - beta) < epsilon or abs(alpha - beta + np.pi) < epsilon or abs(alpha - beta - np.pi) < epsilon):
print("switch to angle mode")
u1 = u2 = 0.0
robot.send_cmd(u1, u2)
self.mode = 'angle'
elif abs(alpha - beta) < epsilon:
# forward
e = np.linalg.norm(x_pred[0:2] - target_pos[0:2]) # compute euclidean distance to target
p = self.P * e
# i += self.I * dt * e # right Riemann sum
i += self.I * dt * (e + e_old) / 2.0 # trapezoidal rule
d = self.D * (e - e_old) / dt
u1 = p - i + d
u2 = u1
else:
# backward
e = np.linalg.norm(x_pred[0:2] - target_pos[0:2]) # compute euclidean distance to target
p = self.P * e
# i += self.I * dt * e # right Riemann sum
i += self.I * dt * (e + e_old) / 2.0 # trapezoidal rule
d = self.D * (e - e_old) / dt
u1 = -(p - i + d)
u2 = u1
e_old = e
#else:
# print("switching to angle mode")
# u1 = u2 = 0.0
# self.mode = 'angle'
elif self.mode == 'combined':
# compute angle such that robot faces to target point
@ -262,22 +217,42 @@ class PIDController:
e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
e_pos = np.linalg.norm(v)
p_angle = self.P_angle * e_angle
i_angle += self.I_angle * dt * (e_angle + e_angle_old) / 2.0 # trapezoidal rule
d_angle = self.D_angle * (e_angle - e_angle_old) / dt
p_pos = self.P_pos * e_pos
i_pos += self.I_pos * dt * (e_pos + e_pos_old) / 2.0 # trapezoidal rule
d_pos = self.D_pos * (e_pos - e_pos_old) / dt
u1 = p_angle + p_pos - i_angle - i_pos
u2 = - p_angle + p_pos + i_angle - i_pos
e_pos_old = e_pos
e_angle_old = e_angle
if e_pos < 0.05:
self.mode = 'angle'
e_angle_old = 0
e_pos_old = 0
i_angle = 0
i_pos = 0
u1 = 0
u2 = 0
else:
forward = abs(e_angle) < np.pi/2.0
if not forward:
if e_angle > np.pi/2.0:
e_angle -= np.pi
else:
e_angle += np.pi
p_angle = self.P_angle * e_angle
i_angle += self.I_angle * dt * (e_angle + e_angle_old) / 2.0 # trapezoidal rule
d_angle = self.D_angle * (e_angle - e_angle_old) / dt
p_pos = self.P_pos * e_pos
i_pos += self.I_pos * dt * (e_pos + e_pos_old) / 2.0 # trapezoidal rule
d_pos = self.D_pos * (e_pos - e_pos_old) / dt
if forward:
print("forward")
u1 = p_angle + p_pos - i_angle - i_pos - d_angle - d_pos
u2 = - p_angle + p_pos + i_angle - i_pos + d_angle - d_pos
else:
print("backward")
u1 = p_angle - p_pos - i_angle + i_pos - d_angle + d_pos
u2 = - p_angle - p_pos + i_angle + i_pos + d_angle + d_pos
e_pos_old = e_pos
e_angle_old = e_angle
else:
u1 = 0.0