File size: 1,932 Bytes
f08920f |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 |
# =========================================================
# الملف 5: simulation/controller.py
# =========================================================
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
|