| |
| |
| |
| 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 |
|
|
