diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py index d983199..b67db67 100644 --- a/remote_control/pid_controller.py +++ b/remote_control/pid_controller.py @@ -2,15 +2,13 @@ import numpy as np import math import time - class PIDController: - def __init__(self, estimator): + def __init__(self, event_listener): self.t = time.time() - self.estimator = estimator 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.D_angle = 0.1 @@ -20,84 +18,17 @@ class PIDController: self.mode = None - def move_to_pos(self, target_pos, robot, near_target_counter=5): - near_target = 0 - while near_target < near_target_counter: - while not self.estimator.event_queue.empty(): - event = self.estimator.event_queue.get() - print("event: ", event) - if event[0] == 'click': - pass - elif event[0] == 'key': - key = event[1] + self.event_listener = event_listener - 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 + def control_multiple_robots(self, robots): + robot_target_positions = {} + for robot in robots: + robot_target_positions[robot.id] = None - x_pred = self.get_measurement(robot.id) + running = True + while running: + pass - 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): controlled_robot_number = 0 @@ -119,11 +50,12 @@ class PIDController: running = True while running: # handle events from opencv window - while not self.estimator.event_queue.empty(): - event = self.estimator.event_queue.get() + while not self.event_listener.event_queue.empty(): + event = self.event_listener.event_queue.get() print("event: ", event) 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_pos = 0 e_pos_old = 0 @@ -181,11 +113,10 @@ class PIDController: if self.controlling: # 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() - - #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 @@ -243,11 +174,9 @@ class PIDController: 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 @@ -260,8 +189,4 @@ class PIDController: #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.1) - - - def get_measurement(self, robot_id): - return self.estimator.get_robot_state_estimate(robot_id) + time.sleep(0.05)