RoboRally/remote_control/pid_controller.py

119 lines
3.8 KiB
Python

import numpy as np
import math
import time
from controller import ControllerBase
class PIDController(ControllerBase):
def __init__(self):
super().__init__()
self.t = None
self.controlling = False
self.P_angle = 0.4
self.I_angle = 0.35
self.D_angle = 0.1
self.P_pos = 0.50
self.I_pos = 0.3
self.D_pos = 0.1
self.mode = 'combined'
self.e_angle_old = 0.0
self.e_pos_old = 0.0
self.i = 0.0
self.i_angle = 0.0
self.i_pos = 0.0
def set_target_position(self, target_pos):
super(PIDController, self).set_target_position(target_pos)
self.mode = 'combined'
self.e_angle_old = 0.0
self.e_pos_old = 0.0
self.i = 0.0
self.i_angle = 0.0
self.i_pos = 0.0
def compute_control(self, state):
# measure state
x_pred = state[1:]
if self.t is None:
dt = 0.1
else:
dt = time.time() - self.t
#print(f"x_pred = {x_pred}\ntarget = {target_pos}\nerror = {np.linalg.norm(target_pos - x_pred)}\n")
if self.mode == 'angle':
# compute angle such that robot faces to target point
target_angle = self.target_pos[2]
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
p = self.P_angle * e_angle
# i += self.I * dt * e # right Riemann sum
self.i += self.I_angle * dt * (e_angle + self.e_angle_old)/2.0 # trapezoidal rule
d = self.D_angle * (e_angle - self.e_angle_old)/dt
u1 = p + self.i + d
u2 = - u1
self.e_angle_old = e_angle
elif self.mode == 'combined':
# compute angle such that robot faces to target point
v = self.target_pos[0:2] - x_pred[0:2]
target_angle = math.atan2(v[1], v[0])
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)
if e_pos < 0.05:
self.mode = 'angle'
self.e_angle_old = 0
self.e_pos_old = 0
self.i_angle = 0
self.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
self.i_angle += self.I_angle * dt * (e_angle + self.e_angle_old) / 2.0 # trapezoidal rule
d_angle = self.D_angle * (e_angle - self.e_angle_old) / dt
p_pos = self.P_pos * e_pos
self.i_pos += self.I_pos * dt * (e_pos + self.e_pos_old) / 2.0 # trapezoidal rule
d_pos = self.D_pos * (e_pos - self.e_pos_old) / dt
if forward:
u1 = p_angle + p_pos + self.i_angle + self.i_pos + d_angle + d_pos
u2 = - p_angle + p_pos - self.i_angle + self.i_pos - d_angle + d_pos
else:
u1 = p_angle - p_pos + self.i_angle - self.i_pos + d_angle - d_pos
u2 = - p_angle - p_pos - self.i_angle - self.i_pos - d_angle - d_pos
self.e_pos_old = e_pos
self.e_angle_old = e_angle
else:
u1 = 0.0
u2 = 0.0
#print(f"u = ({u1}, {u2})")
self.t = time.time() # save time when the most recent control was applied
return (u1, u2)