diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py index b67db67..0a63f3f 100644 --- a/remote_control/pid_controller.py +++ b/remote_control/pid_controller.py @@ -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)