# Source code for scenic.domains.driving.controllers

```"""Low-level controllers useful for vehicles.

The Lateral/Longitudinal PID controllers are adapted from `CARLA`_'s PID controllers,
which are licensed under the following terms:

.. _CARLA: https://carla.org/
"""

from collections import deque

import numpy as np

[docs]class PIDLongitudinalController:
"""Longitudinal control using a PID to reach a target speed.

Arguments:
K_P: Proportional gain
K_D: Derivative gain
K_I: Integral gain
dt: time step
"""

def __init__(self, K_P=0.5, K_D=0.1, K_I=0.2, dt=0.1):
self._k_p = K_P
self._k_d = K_D
self._k_i = K_I
self._dt = dt
self._error_buffer = deque(maxlen=10)

[docs]    def run_step(self, speed_error):
"""Estimate the throttle/brake of the vehicle based on the PID equations.

Arguments:
speed_error: target speed minus current speed

Returns:
a signal between -1 and 1, with negative values indicating braking.
"""
error = speed_error
self._error_buffer.append(error)

if len(self._error_buffer) >= 2:
_de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
_ie = sum(self._error_buffer) * self._dt
else:
_de = 0.0
_ie = 0.0

return np.clip(
(self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0
)

[docs]class PIDLateralController:
"""Lateral control using a PID to track a trajectory.

Arguments:
K_P: Proportional gain
K_D: Derivative gain
K_I: Integral gain
dt: time step
"""

def __init__(self, K_P=0.3, K_D=0.2, K_I=0, dt=0.1):
self.Kp = K_P
self.Kd = K_D
self.Ki = K_I
self.PTerm = 0
self.ITerm = 0
self.DTerm = 0
self.dt = dt
self.last_error = 0
self.windup_guard = 20.0
self.output = 0

[docs]    def run_step(self, cte):
"""Estimate the steering angle of the vehicle based on the PID equations.

Arguments:
cte: cross-track error (distance to right of desired trajectory)

Returns:
a signal between -1 and 1, with -1 meaning maximum steering to the left.
"""
error = cte
delta_error = error - self.last_error
self.PTerm = self.Kp * error
self.ITerm += error * self.dt

if self.ITerm < -self.windup_guard:
self.ITerm = -self.windup_guard
elif self.ITerm > self.windup_guard:
self.ITerm = self.windup_guard

self.DTerm = delta_error / self.dt

# Remember last error for next calculation
self.last_error = error

self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)

return np.clip(self.output, -1, 1)
```