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"
