def long_pid_control(self, next_waypoint: Transform,
control: VehicleControl):
dist_to_car = next_waypoint.location.z
if dist_to_car < self.distance_to_keep:
self.logger.info("TOO CLOSE BRAKING!")
control.brake = True
control.throttle = 0
else:
error = dist_to_car - self.distance_to_keep
error_dt = 0 if len(self.long_error_queue
) == 0 else error - self.long_error_queue[-1]
self.long_error_queue.append(error)
error_it = sum(self.long_error_queue)
e_p = self.lon_kp * error
e_d = self.lon_kd * error_dt
e_i = self.lon_ki * error_it
long_control = np.clip(e_p + e_d + e_i, -1, self.max_throttle)
control.throttle = long_control
def long_pid_control(self, next_waypoint, control: VehicleControl):
self_point = self.agent.vehicle.transform.to_array()
target_point = next_waypoint.location.to_array()
x_diff = target_point[0] - self_point[0]
y_diff = target_point[1] - self_point[1]
z_diff = target_point[2] - self_point[2]
dist_to_car = math.sqrt(x_diff * x_diff + z_diff * z_diff)
if dist_to_car < self.distance_to_keep:
self.logger.info(
f"TOO CLOSE BRAKING! dist_to_car: {dist_to_car} < self.distance_to_keep:{self.distance_to_keep}"
)
control.brake = True
control.throttle = 0
else:
error = dist_to_car - self.distance_to_keep
error_dt = 0 if len(self.long_error_queue
) == 0 else error - self.long_error_queue[-1]
self.long_error_queue.append(error)
error_it = sum(self.long_error_queue)
e_p = self.lon_kp * error
e_d = self.lon_kd * error_dt
e_i = self.lon_ki * error_it
long_control = np.clip(e_p + e_d + e_i, -1, self.max_throttle)
control.throttle = long_control
