def __init__(self, decel_limit, accel_limit, max_steer_angle, max_lat_accel, min_speed, wheel_base, steer_ratio, vehicle_mass, wheel_radius, max_throttle, max_brake): self.decel_limit = decel_limit self.accel_limit = accel_limit self.max_steer_angle = max_steer_angle self.vehicle_mass = vehicle_mass self.wheel_radius = wheel_radius # self.throttle_pid = pid.PID(kp = 0.6, ki = 0.004, kd = 0.2, mn=decel_limit, mx=accel_limit) # self.throttle_pid = pid.PID(kp = 40.0, ki = 0.0, kd = 0.7, mn=max_brake, mx=max_throttle) # percents cte self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle) # cte is m/s self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle) # ki = 0.04 self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.clk = rospy.get_time()
def __init__(self, max_brake, max_throttle, max_steer_angle, wheel_base, steer_ratio, min_speed, max_lat_accel, wheel_radius, vehicle_mass, fuel_capacity): self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle) self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle) self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.yaw_controller = yaw_controller.YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.clk = rospy.get_time() self.max_acc = max_throttle self.max_dec = max_brake self.wheel_radius = wheel_radius self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.max_brk_trq = (vehicle_mass + fuel_capacity * GAS_DENSITY) * wheel_radius * self.max_dec # -1809 N*m
def __init__(self, max_brake, max_throttle, max_steer_angle, wheel_base, steer_ratio, min_speed, max_lat_accel, vehicle_mass, fuel_capacity, brake_deadband, wheel_radius): self.throttle_pid = pid.PID(kp=3, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle) self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle) self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.yaw_controller = yaw_controller.YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.wheel_radius = wheel_radius self.max_throttle = max_throttle self.max_brake = max_brake self.max_brake_torque = (vehicle_mass + fuel_capacity*GAS_DENSITY) * wheel_radius * max_brake self.clk = rospy.get_time() self.count = 0
def __init__(self, *args, **kwargs): # Timesteps self.dt = 0.1 # 10Hz self.prevtime = rospy.get_time() # TODO: Pull PID Gains out to Launch Files # TODO: Tune Coefficients # Initialize Gains on PID Controllers self.PIDCont_Thr = pid.PID(kp=1.0,ki=0.04,kd=0.1,mn= 0.0,mx=1.0) self.PIDCont_Brk = pid.PID(kp=200.0,ki=0.00,kd=0.0,mn= 0.0,mx=10000.0) if STEER_CTE: # cross track error based steering self.PIDCont_Str = pid.PID(kp=0.25,ki=0.075,kd=1.5,mn=-0.5,mx=0.5) else: # angular velocity error based steering self.PIDCont_Str = pid.PID(kp=4.0,ki=0.5,kd=0.05,mn=-0.5,mx=0.5) self.YawCont_Str = yc.YawController(wheel_base=2.8498, steer_ratio=14.8, min_speed=10.0, max_lat_accel=3.0, max_steer_angle=8.0) # Initialize Low Pass Filters self.LPFilt_Thr = lowpass.LowPassFilter(tau=0.0,ts=self.dt) self.LPFilt_Brk = lowpass.LowPassFilter(tau=0.0,ts=self.dt) if STEER_CTE: # cross track error based steering self.LPFilt_Str = lowpass.LowPassFilter(tau=0.7,ts=self.dt) else: # angular velocity error based steering self.LPFilt_Str = lowpass.LowPassFilter(tau=0.5,ts=self.dt) self.first = True # first pass flag pass
def __init__( self, throttle_kp, throttle_ki, throttle_kd, max_speed, accel_limit, decel_limit, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, vehicle_mass, wheel_radius, **kwargs): self.throttle_brake_controller = throttle_brake_controller.ThrottleBrakeController( kp=throttle_kp, ki=throttle_ki, kd=throttle_kd, max_speed=max_speed, accel_limit=accel_limit, decel_limit=decel_limit, brake_coef=vehicle_mass * wheel_radius) self.yaw_controller = yaw_controller.YawController( wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=min_speed, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) self.lowpass_filter = lowpass.LowPassFilter(kwargs.get('tau', 0.2),kwargs.get('ts', 0.1)) self.last_time = None
def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = yaw_controller.YawController( wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. # min and max throttle mn = 0. mx = 0.2 self.throttle_controller = pid.PID(kp, ki, kd, mn, mx) # 1/(2pi*tau) = cutoff freq tau = 0.5 # sample time ts = .02 self.vel_lpf = lowpass.LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() pass
def __init__(self, vehicle_mass, fuel_capacity, accel_limit, decel_limit, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, brake_deadband): # TODO: Implement self.velocity_controller = pid.PID(kp=K_P, ki=K_I, kd=K_D, mn=decel_limit, mx=accel_limit) self.yaw_controller = yaw_controller.YawController( wheel_base=wheel_base, steer_ratio=steer_ratio, min_speed=1., max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) self.yaw_filter = lowpass.LowPassFilter(YAW_TAU, YAW_TS) # need to calculate torque self.total_mass = vehicle_mass + (fuel_capacity * GAS_DENSITY) self.wheel_radius = wheel_radius self.brake_deadband = brake_deadband self.decel_limit = decel_limit
def __init__(self, parmams): self.vehicle_mass = parmams['vehicle_mass'] self.fuel_capacity = parmams['fuel_capacity'] self.brake_deadband = parmams['brake_deadband'] self.decel_limit = parmams['decel_limit'] self.accel_limit = parmams['accel_limit'] self.wheel_radius = parmams['wheel_radius'] self.wheel_base = parmams['wheel_base'] self.steer_ratio = parmams['steer_ratio'] self.max_lat_accel = parmams['max_lat_accel'] self.max_steer_angle = parmams['max_steer_angle'] self.pid_accel = PID.PID(0.3, 0.05, 0.3, mn=self.decel_limit, mx=self.accel_limit) self.yaw_cont = yaw_controller.YawController(self.wheel_base, self.steer_ratio, 0.1, self.max_lat_accel, self.max_steer_angle) self.vel_LP = lowpass.LowPassFilter(0.5, 0.02)
def estimate(self, data, wind_time_factor): print("Estimating winds aloft:") if wind_time_factor: self.wind_time_factor = wind_time_factor self.filt_wn = lowpass.LowPassFilter(self.wind_time_factor, 0.0) self.filt_we = lowpass.LowPassFilter(self.wind_time_factor, 0.0) self.filt_ps = lowpass.LowPassFilter(self.pitot_time_factor, 1.0) self.last_time = 0.0 winds = [] airspeed = 0 psi = 0 vn = 0 ve = 0 wind_deg = 0 wind_kt = 0 ps = 1.0 iter = flight_interp.IterateGroup(data) for i in tqdm(range(iter.size())): record = iter.next() if len(record): t = record['imu']['time'] if 'air' in record: airspeed = record['air']['airspeed'] if 'filter' in record: psi = record['filter']['psi'] vn = record['filter']['vn'] ve = record['filter']['ve'] if airspeed > 10.0: (wn, we, ps) = self.update(t, airspeed, psi, vn, ve) #print wn, we, math.atan2(wn, we), math.atan2(wn, we)*r2d wind_deg = 90 - math.atan2(wn, we) * r2d if wind_deg < 0: wind_deg += 360.0 wind_kt = math.sqrt(we * we + wn * wn) * mps2kt #print wn, we, ps, wind_deg, wind_kt # make sure we log one record per each imu record winds.append({ 'time': t, 'wind_deg': wind_deg, 'wind_kt': wind_kt, 'pitot_scale': ps }) return winds
def __init__(self, max_brake, max_throttle, max_steer_angle, wheel_base, steer_ratio, min_speed, max_lat_accel): self.throttle_pid = pid.PID(kp=1.5, ki=0.0, kd=0.01, mn=max_brake, mx=max_throttle) self.throttle_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.steer_pid = pid.PID(kp=0.5, ki=0.0, kd=0.2, mn=-max_steer_angle, mx=max_steer_angle) self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.yaw_controller = yaw_controller.YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.clk = rospy.get_time()
def __init__(self, wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle, vehicle_mass, brake_deadband, wheel_radius): # TODO: Implement self.dt = 0.0 self.timestamp = time.time() self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.vel_pid = pid.PID(kp=1.1, ki=0.05, kd=0.8, mn=-1, mx=1) self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed, max_lat_accel, max_steer_angle) self.vehicle_mass = vehicle_mass self.brake_deadband = brake_deadband self.wheel_radius = wheel_radius
def __init__(self, max_steer_angle): ms = max_steer_angle # max_steer_angle # create lowpass filters self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) # override filter. It induces lag. Here for reference on how to use self.steer_filter = None # create controllers self.steer_pid = pid.PID(kp=0.5, ki=0.003, kd=0.25, mn=-ms, mx=ms, lowpass_filter=self.steer_filter) # init timestamp self.timestamp = rospy.get_time()
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): kp = 0.45 ki = 0.003 kd = 0.25 mn = -max_steer_angle mx = max_steer_angle tau = 0.5 ts = 0.02 self.vel_lpf = lowpass.LowPassFilter(tau, ts) self.yaw_controller = YawController(wheel_base, steer_ratio, 0.0, max_lat_accel, max_steer_angle) self.steering_correction_pid = pid.PID(kp, ki, kd, mn, mx) self.timestamp = rospy.get_time()
def __init__(self, *args, **kwargs): max_abs_angle = kwargs.get('max_steer_angle') fuel_capacity = kwargs.get('fuel_capacity') self.vehicle_mass = kwargs.get('vehicle_mass') self.vehicle_mass = self.vehicle_mass + (fuel_capacity * GAS_DENSITY) self.wheel_radius = kwargs.get('wheel_radius') self.accel_limit = kwargs.get('accel_limit') self.decel_limit = kwargs.get('decel_limit') self.brake_deadband = kwargs.get('brake_deadband') self.max_acceleration = kwargs.get('max_acceleration') self.wheel_base = kwargs.get('wheel_base') self.steer_ratio = kwargs.get('steer_ratio') self.max_steer_angle = kwargs.get('max_steer_angle') self.max_lat_accel = kwargs.get('max_lat_accel') # PID controllers if use_steering_pid: # create lowpass filters self.steer_filter = lowpass.LowPassFilter(tau=0.0, ts=1.0) self.pid_steer = pid.PID(kp=S_KP, ki=S_KI, kd=S_KD, mn=-max_abs_angle, mx=max_abs_angle) self.pid_velocity = pid.PID(kp=V_KP, ki=V_KI, kd=V_KD) self.yaw_controller = yaw_controller.YawController( self.wheel_base, self.steer_ratio, 1.0, #0.0, self.max_lat_accel, self.max_steer_angle) # aLEX YAW CONTROLLER SET TO 0 # max torque (1.0 throttle) and max brake torque (deceleration lmt) self.max_acc_torque = self.vehicle_mass * self.max_acceleration * self.wheel_radius self.max_brake_torque = self.vehicle_mass * abs( self.decel_limit) * self.wheel_radius # 1st timestamp self.last_time = rospy.get_time()
# Estimate wind vector given indicated airspeed, aircraft heading # (true), and gps ground velocity vector. This function is designed # to be called repeatedly to update the wind estimate in real time. import math import lowpass # useful constants d2r = math.pi / 180.0 r2d = 180.0 / math.pi mps2kt = 1.94384 kt2mps = 1 / mps2kt time_factor = 60 filt_wn = lowpass.LowPassFilter(time_factor, 0.0) filt_we = lowpass.LowPassFilter(time_factor, 0.0) filt_ps = lowpass.LowPassFilter(time_factor * 4.0, 1.0) last_time = 0.0 def update_wind(time, airspeed_kt, yaw_rad, vn, ve): global last_time global filt_wn global filt_we global filt_ps dt = 0.0 if last_time > 0: dt = time - last_time last_time = time
def ResetLPFs(self): self.LPFilt_Thr = lowpass.LowPassFilter(tau=0.0,ts=self.dt) self.LPFilt_Brk = lowpass.LowPassFilter(tau=0.0,ts=self.dt) self.LPFilt_Str = lowpass.LowPassFilter(tau=0.5,ts=self.dt) self.LPFilt_CTE = lowpass.LowPassFilter(tau=0.2,ts=self.dt) pass