|
|
|
|
|
|
|
|
|
|
|
import numpy as np |
|
|
from collections import deque |
|
|
|
|
|
class PIDController: |
|
|
def __init__(self,K_P=1.0,K_I=0.0,K_D=0.0,n=20):self._K_P=K_P;self._K_I=K_I;self._K_D=K_D;self._window=deque([0 for _ in range(n)],maxlen=n) |
|
|
def step(self,error): |
|
|
self._window.append(error);integral=np.mean(self._window);derivative=self._window[-1]-self._window[-2] if len(self._window)>=2 else 0.0 |
|
|
return self._K_P*error+self._K_I*integral+self._K_D*derivative |
|
|
|
|
|
class ControllerConfig: |
|
|
turn_KP,turn_KI,turn_KD,turn_n=1.0,0.1,0.1,20 |
|
|
speed_KP,speed_KI,speed_KD,speed_n=0.5,0.05,0.1,20 |
|
|
max_speed,max_throttle,clip_delta=6.0,0.75,0.25 |
|
|
brake_ratio=1.1 |
|
|
|
|
|
class InterfuserController: |
|
|
def __init__(self,config): |
|
|
self.turn_controller=PIDController(K_P=config.turn_KP,K_I=config.turn_KI,K_D=config.turn_KD,n=config.turn_n) |
|
|
self.speed_controller=PIDController(K_P=config.speed_KP,K_I=config.speed_KI,K_D=config.speed_KD,n=config.speed_n) |
|
|
self.config=config |
|
|
def run_step(self,speed,waypoints,is_junction,traffic_light_state,stop_sign): |
|
|
aim=(waypoints[1]+waypoints[0])/2.0;angle=np.degrees(np.pi/2-np.arctan2(aim[1],aim[0]))/90 |
|
|
if speed<0.01:angle=0 |
|
|
steer=self.turn_controller.step(angle);steer=np.clip(steer,-1.0,1.0) |
|
|
brake=False |
|
|
if(traffic_light_state.sigmoid()[0,0].item()>0.5)or(stop_sign.sigmoid()[0,1].item()>0.5):desired_speed=0.0 |
|
|
else:desired_speed=self.config.max_speed |
|
|
delta=np.clip(desired_speed-speed,0.0,self.config.clip_delta);throttle=self.speed_controller.step(delta) |
|
|
throttle=np.clip(throttle,0.0,self.config.max_throttle) |
|
|
if desired_speed<1e-2 or speed>desired_speed*self.config.brake_ratio:brake=True;throttle=0.0 |
|
|
return steer,throttle,brake |
|
|
|
