Beispiel #1
0
    def __init__(self, vision=False, throttle=True,
                 gear_change=False, port=3001, pid_assist=True,
                 CLIENT_MAX_STEPS=np.inf):
        """Init Method."""
        self.pid_assist = pid_assist
        if self.pid_assist:
            self.action_dim = 2  # LanePos, Velocity
        else:
            self.action_dim = 3  # Accel, Steer, Brake
        TorcsEnv.__init__(self, vision=False, throttle=True, gear_change=False)
        self.state_dim = 29  # No. of sensors input
        self.env_name = 'Madras_Env'
        self.port = port
        self.CLIENT_MAX_STEPS = CLIENT_MAX_STEPS
        self.client_type = 0  # Snakeoil client type
        self.initial_reset = True
        self.early_stop = True
        if self.pid_assist:
            self.PID_latency = 10
        else:
            self.PID_latency = 1
        self.accel_PID = PID(np.array([10.5, 0.05, 2.8]))  # accel PID
        self.steer_PID = PID(np.array([5.1, 0.001, 0.000001]))  # steer PID

        self.prev_lane = 0
        self.prev_angle = 0
        self.prev_vel = 0
        self.prev_dist = 0
        self.ob = None
        self.track_len = 7014.6
    def __init__(self, name, goal, N, net_input, **kwargs):
        super().__init__(**kwargs)

        self.name = name
        self.goal = goal
        self.N = N

        self.p_distributed_controller = PID(5,
                                            0,
                                            0,
                                            max_out=16.6,
                                            min_out=-16.6)
        self.net_input = net_input
    def __init__(self, name, index, p, i, d, **kwargs) -> None:
        super().__init__(**kwargs)

        self.name = name
        self.index = index

        self.initial_position = None
        self.goal_position = None
        self.goal_angle = 0

        self.dictionary = None

        self.colour = None
        self.p_distributed_controller = PID(p,
                                            i,
                                            d,
                                            max_out=16.6,
                                            min_out=-16.6)
Beispiel #4
0
 def __init__(self, attrs, Ts):
     self.kp_phi = 1
     self.kd_phi = 0
     self.ki_phi = 0
     self.delta_a_limit = np.inf
     self.delta_e_limit = np.inf
     self.kd_phi_tau = 0
     self.roll_hold_controller = PID(self.kp_phi, self.ki_phi, self.kd_phi, self.delta_a_limit, Ts, self.kd_phi_tau)
     
     self.kp_chi = 0
     self.ki_chi = 0
     self.heading_hold_controller = PID(self.kp_chi, self.ki_chi, 0, np.inf, Ts * 1.0, 0)
     
     self.kp_theta = 0
     self.ki_theta = 0
     self.pitch_hold_controller = PID(self.kp_theta, self.ki_theta, 0, self.delta_e_limit, Ts, 0)
     
     self.kp_h = 1
     self.ki_h = 0
     self.altitude_hold_controller = PID(self.kp_h, self.ki_h, 0, np.inf, Ts * 1.0, 0)
     
     self.kp_v1 = 1
     self.ki_v1 = 0
     self.airspeed_hold_with_pitch_controller = PID(self.kp_v1, self.ki_v1, 0, np.inf, Ts, 0)
     
     self.kp_v = 1
     self.ki_v = 0
     self.airspeed_hold_with_throttle_controller = PID(self.kp_v, self.ki_v, 0, np.inf, Ts, 0)
Beispiel #5
0
class Autopilot:
    def __init__(self, attrs, Ts):
        self.kp_phi = 1
        self.kd_phi = 0
        self.ki_phi = 0
        self.delta_a_limit = np.inf
        self.delta_e_limit = np.inf
        self.kd_phi_tau = 0
        self.roll_hold_controller = PID(self.kp_phi, self.ki_phi, self.kd_phi, self.delta_a_limit, Ts, self.kd_phi_tau)
        
        self.kp_chi = 0
        self.ki_chi = 0
        self.heading_hold_controller = PID(self.kp_chi, self.ki_chi, 0, np.inf, Ts * 1.0, 0)
        
        self.kp_theta = 0
        self.ki_theta = 0
        self.pitch_hold_controller = PID(self.kp_theta, self.ki_theta, 0, self.delta_e_limit, Ts, 0)
        
        self.kp_h = 1
        self.ki_h = 0
        self.altitude_hold_controller = PID(self.kp_h, self.ki_h, 0, np.inf, Ts * 1.0, 0)
        
        self.kp_v1 = 1
        self.ki_v1 = 0
        self.airspeed_hold_with_pitch_controller = PID(self.kp_v1, self.ki_v1, 0, np.inf, Ts, 0)
        
        self.kp_v = 1
        self.ki_v = 0
        self.airspeed_hold_with_throttle_controller = PID(self.kp_v, self.ki_v, 0, np.inf, Ts, 0)
    
    def compute_delta_a(self, phi_c, phi, *args):
        return self.roll_hold_controller.compute_control_input(phi_c, phi, *args)
    
    def compute_roll(self, chi_c, chi):
        return self.heading_hold_controller.compute_control_input(chi_c, chi)
        
    def compute_delta_e(self, theta_c, theta, *args):
        return self.pitch_hold_controller.compute_control_input(theta_c, theta, *args)
        
    def compute_pitch(self, h_c, h, *args):
        return self.altitude_hold_controller.compute_control_input(h_c, h, *args)
        
    def compute_pitch_for_airspeed(self, Va_c, Va, *args):
        return self.airspeed_hold_with_pitch_controller.compute_control_input(Va_c, Va, *args)
        
    def compute_throttle_for_airspeed(self, Va_c, Va, *args):
        return self.airspeed_hold_with_throttle_controller.compute_control_input(Va_c, Va, *args)
Beispiel #6
0
def playTraffic(port=3101, target_vel=50.0, angle=0.0, sleep=0):
    """Traffic Play function."""
    env = TorcsEnv(vision=False, throttle=True, gear_change=False)
    ob = None
    while ob is None:
        try:
            client = snakeoil3.Client(p=port, vision=False)
            client.MAX_STEPS = np.inf
            client.get_servers_input(step=0)
            obs = client.S.d
            ob = env.make_observation(obs)
        except:
            pass
    episode_count = cfg['traffic']['max_eps']
    max_steps = cfg['traffic']['max_steps_eps']
    early_stop = 0
    velocity = target_vel / 300.0
    accel_pid = PID(np.array([10.5, 0.05, 2.8]))
    steer_pid = PID(np.array([5.1, 0.001, 0.000001]))
    steer = 0.0
    accel = 0.0
    brake = 0
    print(velocity)
    for i in range(episode_count):
        info = {'termination_cause': 0}
        steer = 0.0
        accel = 0.0
        brake = 0

        for step in range(max_steps):
            a_t = np.asarray([steer, accel, brake])  # [steer, accel, brake]
            try:
                ob, r_t, done, info = env.step(step, client, a_t, early_stop)
                if done:
                    break
            except Exception as e:
                print("Exception caught at port " + str(i) + str(e))
                ob = None
                while ob is None:
                    try:
                        client = snakeoil3.Client(p=port, vision=False)
                        client.MAX_STEPS = np.inf
                        client.get_servers_input(step=0)
                        obs = client.S.d
                        ob = env.make_observation(obs)
                    except:
                        pass
                    continue
            if (step <= sleep):
                print("WAIT" + str(port))
                continue
            opp = ob.opponents
            front = np.array([opp[15], opp[16], opp[17], opp[18], opp[19]])
            closest_front = np.min(front)
            print(ob.speedX * 300)
            vel_error = velocity - ob.speedX
            angle_error = -(ob.trackPos + angle
                            ) / 10 + ob.angle + random.choice([1, -1]) * 0.05
            steer_pid.update_error(angle_error)
            accel_pid.update_error(vel_error)
            accel = accel_pid.output()
            steer = steer_pid.output()
            if accel < 0:
                brake = 1
            else:
                brake = 0
            if closest_front < (
                (madras.floatX(0.5 * ob.speedX * 100) + 10.0) / 200.0):
                brake = 1
            else:
                brake = 0

        try:
            if 'termination_cause' in info.keys(
            ) and info['termination_cause'] == 'hardReset':
                print('Hard reset by some agent')
                ob, client = env.reset(client=client, relaunch=True)

        except Exception as e:
            print("Exception caught at point B at port " + str(i) + str(e))
            ob = None
            while ob is None:
                try:
                    client = snakeoil3.Client(
                        p=port, vision=False)  # Open new UDP in vtorcs
                    client.MAX_STEPS = np.inf
                    client.get_servers_input(
                        0)  # Get the initial input from torcs
                    obs = client.S.d  # Get the current full-observation from torcs
                    ob = env.make_observation(obs)
                except:
                    print("Exception caught at at point C at port " + str(i) +
                          str(e))
Beispiel #7
0
 def __init__(self, attrs, Ts):
     self.kp_phi = 1
     self.kd_phi = 0
     self.ki_phi = 0
     self.delta_a_limit = np.inf
     self.delta_e_limit = np.inf
     self.kd_phi_tau = 0
     self.roll_hold_controller = PID(self.kp_phi, self.ki_phi, self.kd_phi, self.delta_a_limit, Ts, self.kd_phi_tau)
     
     self.kp_chi = 0
     self.ki_chi = 0
     self.heading_hold_controller = PID(self.kp_chi, self.ki_chi, 0, np.inf, Ts * 1.0, 0)
     
     self.kp_theta = 0
     self.ki_theta = 0
     self.pitch_hold_controller = PID(self.kp_theta, self.ki_theta, 0, self.delta_e_limit, Ts, 0)
     
     self.kp_h = 1
     self.ki_h = 0
     self.altitude_hold_controller = PID(self.kp_h, self.ki_h, 0, np.inf, Ts * 1.0, 0)
     
     self.kp_v1 = 1
     self.ki_v1 = 0
     self.airspeed_hold_with_pitch_controller = PID(self.kp_v1, self.ki_v1, 0, np.inf, Ts, 0)
     
     self.kp_v = 1
     self.ki_v = 0
     self.airspeed_hold_with_throttle_controller = PID(self.kp_v, self.ki_v, 0, np.inf, Ts, 0)
     
     self.config = attrs
Beispiel #8
0
class Autopilot(object):
    __metaclass__ = ABCMeta
    def __init__(self, attrs, Ts):
        self.kp_phi = 1
        self.kd_phi = 0
        self.ki_phi = 0
        self.delta_a_limit = np.inf
        self.delta_e_limit = np.inf
        self.kd_phi_tau = 0
        self.roll_hold_controller = PID(self.kp_phi, self.ki_phi, self.kd_phi, self.delta_a_limit, Ts, self.kd_phi_tau)
        
        self.kp_chi = 0
        self.ki_chi = 0
        self.heading_hold_controller = PID(self.kp_chi, self.ki_chi, 0, np.inf, Ts * 1.0, 0)
        
        self.kp_theta = 0
        self.ki_theta = 0
        self.pitch_hold_controller = PID(self.kp_theta, self.ki_theta, 0, self.delta_e_limit, Ts, 0)
        
        self.kp_h = 1
        self.ki_h = 0
        self.altitude_hold_controller = PID(self.kp_h, self.ki_h, 0, np.inf, Ts * 1.0, 0)
        
        self.kp_v1 = 1
        self.ki_v1 = 0
        self.airspeed_hold_with_pitch_controller = PID(self.kp_v1, self.ki_v1, 0, np.inf, Ts, 0)
        
        self.kp_v = 1
        self.ki_v = 0
        self.airspeed_hold_with_throttle_controller = PID(self.kp_v, self.ki_v, 0, np.inf, Ts, 0)
        
        self.config = attrs
    
    def compute_delta_a(self, phi_c, phi, *args):
        return self.roll_hold_controller.compute_control_input(phi_c, phi, *args)
    
    def compute_roll(self, chi_c, chi):
        return self.heading_hold_controller.compute_control_input(chi_c, chi)
        
    def compute_delta_e(self, theta_c, theta, *args):
        return self.pitch_hold_controller.compute_control_input(theta_c, theta, *args)
        
    def compute_pitch(self, h_c, h, *args):
        return self.altitude_hold_controller.compute_control_input(h_c, h, *args)
        
    def compute_pitch_for_airspeed(self, Va_c, Va, *args):
        return self.airspeed_hold_with_pitch_controller.compute_control_input(Va_c, Va, *args)
        
    def compute_throttle_for_airspeed(self, Va_c, Va, *args):
        return self.airspeed_hold_with_throttle_controller.compute_control_input(Va_c, Va, *args)
    
    @abstractmethod
    def set_pitch(self, pitch_c): pass

    @abstractmethod
    def set_roll(self, roll_c): pass

    @abstractmethod
    def get_roll_for_heading(self, chi_c): pass
    
    @abstractmethod
    def get_throttle_for_airspeed(self, Va_c): pass

    @abstractmethod
    def get_pitch_for_airspeed(self, Va_c): pass

    @abstractmethod
    def get_pitch_for_altitude(self, h_c): pass      
  
    @abstractmethod
    def set_throttle(self, throttle_c): pass        
class ManualController:
    """
    The robots are moved following a “distributed controller”, that is a simple proportional controller PID,
    with P fixed to -0.01, whose goal is to align the robots by minimizing the difference between the values
    ​​recorded by the front and rear sensors.

    :param name: name of the controller used (in this case manual)
    :param goal: task to perform (in this case distribute)
    :param N: number of agents in the simulation
    :param net_input: input of the network (between: prox_values, prox_comm and all_sensors)
    :param kwargs: other arguments

    :var p_distributed_controller: a simple proportional controller that returns the speed to apply
    """
    def __init__(self, name, goal, N, net_input, **kwargs):
        super().__init__(**kwargs)

        self.name = name
        self.goal = goal
        self.N = N

        self.p_distributed_controller = PID(5,
                                            0,
                                            0,
                                            max_out=16.6,
                                            min_out=-16.6)
        self.net_input = net_input

    def neighbors_distance(self, state):
        """
        Check if there is a robot ahead using the infrared sensor 2 (front-front)
        [2 and 9 in case of all sensors].
        Check if there is a robot ahead using the infrared sensor 5 (back-left) and 6 (back-right)
        [5, 6 and 12, 13 in case of all sensors].
        The distance from the neighbors is computed using a mapping function obtained experimentally that converts the
        intensities values contained in the sensor readings into distances.

        .. note:: The response values are actually intensities: the front correspond to the
                  frontal center sensor and the back to the mean of the response values of the rear sensors.


        :param state: object containing the agent information
        :return back, front: back distance and front distance of the thymio from the others
        """
        sensing = utils.get_input_sensing(self.net_input,
                                          state,
                                          normalise=False)

        if sensing == [0] * len(sensing):
            return 0, 0

        file = os.path.join('source/controllers', 'sensing_to_distances.pkl')
        distances, front_prox_values, back_prox_values, front_prox_comm, back_prox_comm = np.load(
            file, allow_pickle=True)

        f_pv_indices = np.nonzero(front_prox_values)[0]
        f_pv_indices = f_pv_indices[np.argsort(
            front_prox_values[f_pv_indices])]

        b_pv_indices = np.nonzero(back_prox_values)[0]
        b_pv_indices = b_pv_indices[np.argsort(back_prox_values[b_pv_indices])]

        f_pc_indices = np.nonzero(front_prox_comm)[0]
        f_pc_indices = f_pc_indices[np.argsort(front_prox_comm[f_pc_indices])]

        b_pc_indices = np.nonzero(back_prox_comm)[0]
        b_pc_indices = b_pc_indices[np.argsort(back_prox_comm[b_pc_indices])]

        front_distance_pv = np.interp(sensing[2],
                                      front_prox_values[f_pv_indices],
                                      distances[f_pv_indices])
        back_distance_pv = np.interp(
            np.mean(np.array([sensing[5], sensing[6]])),
            back_prox_values[b_pv_indices], distances[b_pv_indices])

        if sensing[7:] == [0] * 7 or self.net_input != 'all_sensors':
            return back_distance_pv, front_distance_pv

        elif sensing[0:7] == [0] * 7:
            front_distance_pc = np.interp(sensing[9],
                                          front_prox_comm[f_pc_indices],
                                          distances[f_pc_indices])
            back_distance_pc = np.interp(
                np.mean(np.array([sensing[12], sensing[13]])),
                back_prox_comm[b_pc_indices], distances[b_pc_indices])
            return back_distance_pc, front_distance_pc

        elif self.net_input == 'all_sensors':
            front_distance_pc = np.interp(sensing[9],
                                          front_prox_comm[f_pc_indices],
                                          distances[f_pc_indices])
            back_distance_pc = np.interp(
                np.mean(np.array([sensing[12], sensing[13]])),
                back_prox_comm[b_pc_indices], distances[b_pc_indices])

            if sensing[2] == 0.0:
                front = front_distance_pc
            else:
                front = front_distance_pv

            if np.mean([sensing[5], sensing[6]]) == 0.0:
                back = back_distance_pc
            elif np.mean([sensing[12], sensing[13]]) == 0.0:
                back = back_distance_pv
            else:
                back = np.mean([back_distance_pv, back_distance_pc])

            return back, front
        else:
            raise ValueError('Impossible values for sensing.')

    def compute_difference(self, state):
        """
        .. note:: Apply a small correction to the distance measured by the rear sensors since the front sensor used
                  is at a different x coordinate from the point to which the rear sensor of the robot that follows
                  points. This is because of the curved shape of the face of the Thymio.
                  The final difference is computed as follow:

                  .. math:: out = front - (back - correction)



        :param state: object containing the agent information
        :return out: the difference between the front and the rear distances
        """

        back, front = self.neighbors_distance(state)
        if back == 0 and front == 0:
            return 0

        # compute the correction
        delta_x = 7.41150769
        x = 7.95

        # Maximum possible response values
        # delta_x_m = 4505 * delta_x / 14
        # x_m = 4505 * x / 14
        # correction = x_m - delta_x_m
        correction = x - delta_x

        out = front - (back - correction)

        return out

    def perform_control(self, state, dt):
        """
        Keeping still the robots at the ends of the line, moves the others using the distributed controller,
        setting the ``target {left, right} wheel speed`` each at the same value in order to moves the robot
        straight ahead.
        This distributed controller is a simple proportional controller ``PID(5, 0, 0, max_out=16.6, min_out=-16.6)``
        that takes in input the difference between the front and back distances measured by the agent.

        :param state: object containing the agent information
        :param dt: control step duration

        :return speed, communication: the velocity and the message to communicate

        """
        # Don't move the first and last robots in the line
        # if state.index == 0 or state.index == self.N - 1:
        if np.isclose(round(state.goal_position[0], 2),
                      round(state.initial_position[0], 2),
                      rtol=1e-2):
            return 0, 0
        else:
            speed = self.p_distributed_controller.step(
                self.compute_difference(state), dt)
            return speed, 0
Beispiel #10
0
class MadrasEnv(TorcsEnv):
    """Definition of the Gym Madras Env."""
    def __init__(self, vision=False, throttle=True,
                 gear_change=False, port=3001, pid_assist=True,
                 CLIENT_MAX_STEPS=np.inf):
        """Init Method."""
        self.pid_assist = pid_assist
        if self.pid_assist:
            self.action_dim = 2  # LanePos, Velocity
        else:
            self.action_dim = 3  # Accel, Steer, Brake
        TorcsEnv.__init__(self, vision=False, throttle=True, gear_change=False)
        self.state_dim = 29  # No. of sensors input
        self.env_name = 'Madras_Env'
        self.port = port
        self.CLIENT_MAX_STEPS = CLIENT_MAX_STEPS
        self.client_type = 0  # Snakeoil client type
        self.initial_reset = True
        self.early_stop = True
        if self.pid_assist:
            self.PID_latency = 10
        else:
            self.PID_latency = 1
        self.accel_PID = PID(np.array([10.5, 0.05, 2.8]))  # accel PID
        self.steer_PID = PID(np.array([5.1, 0.001, 0.000001]))  # steer PID

        self.prev_lane = 0
        self.prev_angle = 0
        self.prev_vel = 0
        self.prev_dist = 0
        self.ob = None
        self.track_len = 7014.6

    def reset(self, prev_step_info=None):
        """Reset Method. To be called at the end of each episode"""
        if self.initial_reset:
            while self.ob is None:
                try:
                    self.client = snakeoil3.Client(p=self.port,
                                                   vision=self.vision)
                    # Open new UDP in vtorcs
                    self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
                    self.client.get_servers_input(step=0)
                    # Get the initial input from torcs
                    raw_ob = self.client.S.d
                    # Get the current full-observation
                    self.ob = self.make_observation(raw_ob)
                except:
                    pass
            self.initial_reset = False

        else:
            try:
                if 'termination_cause' in list(prev_step_info.keys()) and\
                        prev_step_info['termination_cause'] == 'hardReset':
                    self.ob, self.client =\
                        TorcsEnv.reset(self, client=self.client, relaunch=True)
                else:
                    self.ob, self.client =\
                        TorcsEnv.reset(self, client=self.client, relaunch=True)

            except Exception as e:
                self.ob = None
                while self.ob is None:
                    try:
                        self.client = snakeoil3.Client(p=self.port,
                                                       vision=self.vision)
                        # Open new UDP in vtorcs
                        self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
                        self.client.get_servers_input(step=0)
                        # Get the initial input from torcs
                        raw_ob = self.client.S.d
                        # Get the current full-observation from torcs
                        self.ob = self.make_observation(raw_ob)
                    except:
                        pass

        self.distance_traversed = 0
        s_t = np.hstack((self.ob.angle, self.ob.track, self.ob.trackPos,
                        self.ob.speedX, self.ob.speedY, self.ob.speedZ,
                        self.ob.wheelSpinVel / 100.0, self.ob.rpm))

        return s_t

    def step(self, desire):
        """Step method to be called at each time step."""
        r_t = 0

        for PID_step in range(self.PID_latency):
                # Implement the desired trackpos and velocity using PID
            if self.pid_assist:
                self.accel_PID.update_error((desire[1] - self.prev_vel))
                self.steer_PID.update_error((-(self.prev_lane - desire[0]) / 10 +
                                            self.prev_angle))
                if self.accel_PID.output() < 0.0:
                    brake = 1
                else:
                    brake = 0
                a_t = np.asarray([self.steer_PID.output(),
                                 self.accel_PID.output(), brake])
            else:
                a_t = desire
            try:
                self.ob, r, done, info = TorcsEnv.step(self, PID_step,
                                                       self.client, a_t,
                                                       self.early_stop)
            except Exception as e:
                print(("Exception caught at port " + str(e)))
                self.ob = None
                while self.ob is None:
                    try:
                        self.client = snakeoil3.Client(p=self.port,
                                                       vision=self.vision)
                        # Open new UDP in vtorcs
                        self.client.MAX_STEPS = self.CLIENT_MAX_STEPS
                        self.client.get_servers_input(0)
                        # Get the initial input from torcs
                        raw_ob = self.client.S.d
                        # Get the current full-observation from torcs
                        self.ob = self.make_observation(raw_ob)
                    except:
                        pass
                    continue
            self.prev_vel = self.ob.speedX
            self.prev_angle = self.ob.angle
            self.prev_lane = self.ob.trackPos
            if (math.isnan(r)):
                r = 0.0
            r_t += r  # accumulate rewards over all the time steps

            self.distance_traversed = self.client.S.d['distRaced']
            r_t += (self.distance_traversed - self.prev_dist) /\
                self.track_len
            self.prev_dist = deepcopy(self.distance_traversed)
            if self.distance_traversed >= self.track_len:
                done = True
            if done:
                break

        s_t1 = np.hstack((self.ob.angle, self.ob.track, self.ob.trackPos,
                          self.ob.speedX, self.ob.speedY, self.ob.speedZ,
                          self.ob.wheelSpinVel / 100.0, self.ob.rpm))

        return s_t1, r_t, done, info
class Thymio(pyenki.Thymio2):
    """
    Superclass: `pyenki.Thymio2` -> the world update step will automatically call the Thymio `controlStep`.

    :param name: name of the agent
    :param index: index of the agents in the row
    :param p: proportional term of the PID controller
    :param i: integral term of the PID controller
    :param d: derivative term of the PID controller
    :param kwargs: other arguments

    :var initial_position: the initial position of the agent is set to None
    :var goal_position: the goal position of the agent is set to None
    :var goal_angle: the goal angle of the agent is set to None
    :var dictionary: the dictionary containing all the agent attributes is set to None
    :var colour: the colour of the agent is set to None
    :var p_distributed_controller: a simple proportional controller that returns the speed to apply
    """
    def __init__(self, name, index, p, i, d, **kwargs) -> None:
        super().__init__(**kwargs)

        self.name = name
        self.index = index

        self.initial_position = None
        self.goal_position = None
        self.goal_angle = 0

        self.dictionary = None

        self.colour = None
        self.p_distributed_controller = PID(p,
                                            i,
                                            d,
                                            max_out=16.6,
                                            min_out=-16.6)

    def get_input_sensing(self):
        """
        :return sensing: the sensing perceived by the robot based on the net input
        """

        if len(self.prox_comm_events) == 0:
            prox_comm = {'sender': {'intensities': [0, 0, 0, 0, 0, 0, 0]}}
        else:
            prox_comm = utils.get_prox_comm(self)

        prox_values = getattr(self, 'prox_values').copy()

        sensing = utils.get_all_sensors(prox_values, prox_comm)

        return sensing

    def controlStep(self, dt: float) -> None:
        """
        Perform one control step:
            Compute the error as the difference between the goal and the actual position
            and use it to compute the velocity of the robot through a proportional controller.

        :param dt: control step duration
        """
        error = self.goal_position[0] - self.position[0]

        speed = self.p_distributed_controller.step(error, dt)

        self.motor_right_target = speed
        self.motor_left_target = speed