class FollowerControlSensor(FollowerControlData): def __init__(self, iterations: int, connection: Connection, headway: float, leader, noise, sensor_delay): super().__init__(iterations, connection, headway) self.connection = connection self.sensor = Sensor(sensor_delay, noise) self.leader = leader def tick(self, frame: int, time_passed: float): packets = self.connection.read() for packet in packets: if isinstance(packet, PacketTodo): if packet.timestamp < frame: self.pid.setpoint = packet.v else: self.todo[packet.timestamp] = packet if self.todo[frame]: self.pid.setpoint = self.todo[frame].v self.last_todo = frame self.last_packet = self.sensor.get_info(self.leader, frame) if self.last_packet is not None: p = self.last_packet self.pid2.setpoint = calc_ref_v(self.headway, self.x[frame - 1], p.a, p.v, p.x, (frame - p.timestamp - 1) * MILLISECOND) super(FollowerControlData, self).tick(frame, time_passed) def drive(self, frame: int, time_passed: float): self.pid.compute(self.v[frame - 1]) self.pid2.compute(self.v[frame - 1]) sum = self.motor.get_acceleration(self.v[frame - 1], self.pid.output) sum += self.motor.get_acceleration(self.v[frame - 1], self.pid2.output) if sum > 4: sum = 4 elif sum < -8: sum = -8 self.a[frame] = sum def get_name(self): return "Future + Sensor"
class FollowerSensor(CarPID): def __init__(self, iterations, leader: Car, headway: float, max_noise: float, delay: int): super().__init__(iterations, headway) self.pid.set_tuning(40.5, 0, 0.1, 1 * MILLISECOND) self.leader = leader self.sensor = Sensor(delay, max_noise) def tick(self, frame: int, time_passed: float): last_info = self.sensor.get_info(self.leader, frame) if last_info: self.pid.setpoint = calc_ref_v( self.headway, self.x[frame - 1], last_info.a, last_info.v, last_info.x, (frame - last_info.timestamp - 1) * MILLISECOND) super().tick(frame, time_passed) def get_name(self): return "Sensor"