rewrote pid controller to derive from ControllerBase. works with multiple robots now
parent
2060d8eb15
commit
35b6c3bdfd
|
|
@ -2,9 +2,12 @@ import numpy as np
|
|||
import math
|
||||
import time
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, event_listener):
|
||||
self.t = time.time()
|
||||
from controller import ControllerBase
|
||||
|
||||
class PIDController(ControllerBase):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.t = None
|
||||
|
||||
self.controlling = False
|
||||
|
||||
|
|
@ -16,177 +19,100 @@ class PIDController:
|
|||
self.I_pos = 0.3
|
||||
self.D_pos = 0.1
|
||||
|
||||
self.mode = None
|
||||
self.mode = 'combined'
|
||||
|
||||
self.event_listener = event_listener
|
||||
self.e_angle_old = 0.0
|
||||
self.e_pos_old = 0.0
|
||||
|
||||
def control_multiple_robots(self, robots):
|
||||
robot_target_positions = {}
|
||||
for robot in robots:
|
||||
robot_target_positions[robot.id] = None
|
||||
self.i = 0.0
|
||||
self.i_angle = 0.0
|
||||
self.i_pos = 0.0
|
||||
|
||||
running = True
|
||||
while running:
|
||||
pass
|
||||
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 interactive_control(self, robots):
|
||||
controlled_robot_number = 0
|
||||
robot = robots[controlled_robot_number]
|
||||
def compute_control(self, state):
|
||||
# measure state
|
||||
x_pred = state[1:]
|
||||
|
||||
ts = []
|
||||
angles = []
|
||||
if self.t is None:
|
||||
dt = 0.1
|
||||
else:
|
||||
dt = time.time() - self.t
|
||||
|
||||
target_pos = np.array([0.0, 0.0, 0.0])
|
||||
e_angle_old = 0.0
|
||||
e_pos_old = 0.0
|
||||
#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]
|
||||
|
||||
i = 0.0
|
||||
i_angle = 0.0
|
||||
i_pos = 0.0
|
||||
angles_unwrapped = np.unwrap([x_pred[2], target_angle]) # unwrap angle to avoid jump in data
|
||||
|
||||
t0 = time.time()
|
||||
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
|
||||
|
||||
running = True
|
||||
while running:
|
||||
# handle events from opencv window
|
||||
while not self.event_listener.event_queue.empty():
|
||||
event = self.event_listener.event_queue.get()
|
||||
print("event: ", event)
|
||||
if event[0] == 'click':
|
||||
target = event[1]
|
||||
target_pos = np.array([target['x'], target['y'], target['angle']])
|
||||
i_angle = 0
|
||||
i_pos = 0
|
||||
e_pos_old = 0
|
||||
e_angle_old = 0
|
||||
self.mode = 'combined'
|
||||
elif event[0] == 'key':
|
||||
key = event[1]
|
||||
u1 = p + self.i + d
|
||||
u2 = - u1
|
||||
|
||||
if key == 32: # arrow up
|
||||
self.controlling = not self.controlling
|
||||
if not self.controlling:
|
||||
print("disable control")
|
||||
robot.send_cmd() # stop robot
|
||||
self.mode = None # reset control mode
|
||||
else:
|
||||
print("enable control")
|
||||
i = 0
|
||||
self.t = time.time()
|
||||
elif key == 48: # 0
|
||||
target_pos = np.array([0.0, 0.0, 0.0]) # TODO: use center of board for target pos
|
||||
elif key == 97: # a
|
||||
self.mode = 'angle'
|
||||
e_angle_old = 0
|
||||
i = 0
|
||||
self.t = time.time()
|
||||
elif key == 99: # c
|
||||
self.mode = 'combined'
|
||||
e_angle_old = 0
|
||||
e_pos_old = 0
|
||||
i_angle = 0
|
||||
i_pos = 0
|
||||
self.t = time.time()
|
||||
elif key == 112: # p
|
||||
self.mode = 'position'
|
||||
e_pos_old = 0
|
||||
i = 0
|
||||
self.t = time.time()
|
||||
elif key == 43: # +
|
||||
self.P_pos += 0.1
|
||||
print("P = ", self.P_angle)
|
||||
elif key == 45: # -
|
||||
self.P_pos -= 0.1
|
||||
print("P = ", self.P_angle)
|
||||
elif key == 9: # TAB
|
||||
# switch controlled robot
|
||||
robot.send_cmd() # stop current robot
|
||||
controlled_robot_number = (controlled_robot_number + 1) % len(robots)
|
||||
robot = robots[controlled_robot_number]
|
||||
print(f"controlled robot: {robot.id}")
|
||||
elif key == 113 or key == 27: # q or ESCAPE
|
||||
print("quit!")
|
||||
self.controlling = False
|
||||
robot.send_cmd()
|
||||
return
|
||||
self.e_angle_old = e_angle
|
||||
|
||||
if self.controlling:
|
||||
# measure state
|
||||
t, x, y, angle = robot.get_measurement()
|
||||
x_pred = np.array([x, y, angle])
|
||||
dt = self.t - time.time()
|
||||
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])
|
||||
|
||||
#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 = target_pos[2]
|
||||
angles_unwrapped = np.unwrap([x_pred[2], target_angle]) # unwrap angle to avoid jump in data
|
||||
|
||||
ts.append(time.time() - t0)
|
||||
angles.append(x_pred[2])
|
||||
e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
|
||||
e_pos = np.linalg.norm(v)
|
||||
|
||||
angles_unwrapped = np.unwrap([x_pred[2], target_angle]) # unwrap angle to avoid jump in data
|
||||
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
|
||||
|
||||
e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
|
||||
p = self.P_angle * e_angle
|
||||
# i += self.I * dt * e # right Riemann sum
|
||||
i += self.I_angle * dt * (e_angle + e_angle_old)/2.0 # trapezoidal rule
|
||||
d = self.D_angle * (e_angle - e_angle_old)/dt
|
||||
|
||||
u1 = p - i - d
|
||||
u2 = - u1
|
||||
|
||||
e_angle_old = e_angle
|
||||
|
||||
elif self.mode == 'combined':
|
||||
# compute angle such that robot faces to target point
|
||||
v = 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'
|
||||
e_angle_old = 0
|
||||
e_pos_old = 0
|
||||
i_angle = 0
|
||||
i_pos = 0
|
||||
u1 = 0
|
||||
u2 = 0
|
||||
if not forward:
|
||||
if e_angle > np.pi/2.0:
|
||||
e_angle -= np.pi
|
||||
else:
|
||||
forward = abs(e_angle) < np.pi/2.0
|
||||
e_angle += np.pi
|
||||
|
||||
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_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:
|
||||
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:
|
||||
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
|
||||
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 = 0.0
|
||||
u2 = 0.0
|
||||
#print(f"u = ({u1}, {u2})")
|
||||
robot.send_cmd(u1, u2)
|
||||
self.t = time.time() # save time when the most recent control was applied
|
||||
time.sleep(0.05)
|
||||
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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue