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

about to make changes s.t. PID controller derives from ControllerBase class

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-11-14 15:22:41 +01:00
parent 6b9373cce5
commit 47f652fd62
1 changed files with 17 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
remote_control/pid_controller.py
View File

@ -2,15 +2,13 @@ import numpy as np
import math import math
import time import time
class PIDController: class PIDController:
def __init__(self, estimator): def __init__(self, event_listener):
self.t = time.time() self.t = time.time()
self.estimator = estimator
self.controlling = False self.controlling = False
self.P_angle = 0.4 # PID parameters determined by Ziegler-Nichols. measured K_crit = 1.4, T_crit = 1.5 self.P_angle = 0.4
self.I_angle = 0.35 self.I_angle = 0.35
self.D_angle = 0.1 self.D_angle = 0.1
@ -20,84 +18,17 @@ class PIDController:
self.mode = None self.mode = None
def move_to_pos(self, target_pos, robot, near_target_counter=5): self.event_listener = event_listener
near_target = 0
while near_target < near_target_counter: def control_multiple_robots(self, robots):
while not self.estimator.event_queue.empty(): robot_target_positions = {}
event = self.estimator.event_queue.get() for robot in robots:
print("event: ", event) robot_target_positions[robot.id] = None
if event[0] == 'click':
running = True
while running:
pass pass
elif event[0] == 'key':
key = event[1]
if key == 84: # arrow up
self.controlling = True
self.t = time.time()
elif key == 82: # arrow down
self.controlling = False
robot.send_cmd()
elif key == 48: # 0
target_pos = np.array([0.0, 0.0, 0.0])
elif key == 43: # +
self.control_scaling += 0.1
self.control_scaling = min(self.control_scaling, 1.0)
print("control scaling = ", self.control_scaling)
elif key == 45: # -
self.control_scaling -= 0.1
self.control_scaling = max(self.control_scaling, 0.1)
print("control scaling = ", self.control_scaling)
elif key == 113:
print("quit!")
self.controlling = False
robot.send_cmd()
return
elif key == 27: # escape
print("quit!")
self.controlling = False
robot.send_cmd()
return
x_pred = self.get_measurement(robot.id)
if x_pred is not None:
error_pos = np.linalg.norm(x_pred[0:2] - target_pos[0:2])
angles_unwrapped = np.unwrap([x_pred[2], target_pos[2]]) # unwrap angle to avoid jump in data
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
# print("error pos = ", error_pos)
# print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
# if error_pos > 0.075 or error_ang > 0.35:
if error_pos > 0.05 or error_ang > 0.1:
# solve mpc open loop problem
res = self.ols.solve(x_pred, target_pos)
# us1 = res[0]
# us2 = res[1]
us1 = res[0] * self.control_scaling
us2 = res[1] * self.control_scaling
# print("u = {}", (us1, us2))
# print("---------------- mpc solution took {} seconds".format(tmpc_end - tmpc_start))
dt_mpc = time.time() - self.t
if dt_mpc < self.dt: # wait until next control can be applied
# print("sleeping for {} seconds...".format(self.dt - dt_mpc))
time.sleep(self.dt - dt_mpc)
else:
us1 = [0] * self.mstep
us2 = [0] * self.mstep
near_target += 1
# send controls to the robot
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
u1 = us1[i]
u2 = us2[i]
robot.send_cmd(u1, u2)
if i < self.mstep:
time.sleep(self.dt)
self.t = time.time() # save time the most recent control was applied
else:
print("robot not detected yet!")
def interactive_control(self, robots): def interactive_control(self, robots):
controlled_robot_number = 0 controlled_robot_number = 0
@ -119,11 +50,12 @@ class PIDController:
running = True running = True
while running: while running:
# handle events from opencv window # handle events from opencv window
while not self.estimator.event_queue.empty(): while not self.event_listener.event_queue.empty():
event = self.estimator.event_queue.get() event = self.event_listener.event_queue.get()
print("event: ", event) print("event: ", event)
if event[0] == 'click': if event[0] == 'click':
target_pos = event[1] target = event[1]
target_pos = np.array([target['x'], target['y'], target['angle']])
i_angle = 0 i_angle = 0
i_pos = 0 i_pos = 0
e_pos_old = 0 e_pos_old = 0
@ -181,11 +113,10 @@ class PIDController:
if self.controlling: if self.controlling:
# measure state # measure state
x_pred = self.get_measurement(robot.id) t, x, y, angle = robot.get_measurement()
x_pred = np.array([x, y, angle])
dt = self.t - time.time() dt = self.t - time.time()
#print(f"x_pred = {x_pred}\ntarget = {target_pos}\nerror = {np.linalg.norm(target_pos - x_pred)}\n") #print(f"x_pred = {x_pred}\ntarget = {target_pos}\nerror = {np.linalg.norm(target_pos - x_pred)}\n")
if self.mode == 'angle': if self.mode == 'angle':
# compute angle such that robot faces to target point # compute angle such that robot faces to target point
@ -243,11 +174,9 @@ class PIDController:
d_pos = self.D_pos * (e_pos - e_pos_old) / dt d_pos = self.D_pos * (e_pos - e_pos_old) / dt
if forward: if forward:
print("forward")
u1 = p_angle + p_pos - i_angle - i_pos - d_angle - d_pos 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 u2 = - p_angle + p_pos + i_angle - i_pos + d_angle - d_pos
else: else:
print("backward")
u1 = p_angle - p_pos - i_angle + i_pos - d_angle + d_pos 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 u2 = - p_angle - p_pos + i_angle + i_pos + d_angle + d_pos
@ -260,8 +189,4 @@ class PIDController:
#print(f"u = ({u1}, {u2})") #print(f"u = ({u1}, {u2})")
robot.send_cmd(u1, u2) robot.send_cmd(u1, u2)
self.t = time.time() # save time when the most recent control was applied self.t = time.time() # save time when the most recent control was applied
time.sleep(0.1) time.sleep(0.05)
def get_measurement(self, robot_id):
return self.estimator.get_robot_state_estimate(robot_id)