def __init__(self, CP): self.angle_steers_des = 0. A = np.matrix([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL], [0.0, 0.0, 1.0]]) C = np.matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) # K = np.transpose(K) K = np.matrix([[7.30262179e-01, 2.07003658e-04], [7.29394177e+00, 1.39159419e-02], [1.71022442e+01, 3.38495381e-02]]) self.K = K self.A_K = A - np.dot(K, C) self.x = np.matrix([[0.], [0.], [0.]]) self.enforce_rate_limit = CP.carName == "toyota" self.RC = CP.lateralTuning.indi.timeConstant self.G = CP.lateralTuning.indi.actuatorEffectiveness self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.reset() self.tune = nTune(CP, self)
def __init__(self, CP, CI): super().__init__(CP, CI) self.angle_steers_des = 0. A = np.array([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL], [0.0, 0.0, 1.0]]) C = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) # K = np.transpose(K) K = np.array([[7.30262179e-01, 2.07003658e-04], [7.29394177e+00, 1.39159419e-02], [1.71022442e+01, 3.38495381e-02]]) self.speed = 0. self.K = K self.A_K = A - np.dot(K, C) self.x = np.array([[0.], [0.], [0.]]) self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) self.tune = nTune(CP, self) self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL) self.reset()
def __init__(self, CP, CI): super().__init__(CP, CI) self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, k_d=CP.lateralTuning.torque.kd, k_f=CP.lateralTuning.torque.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle self.friction = CP.lateralTuning.torque.friction self.kf = CP.lateralTuning.torque.kf self.deadzone = CP.lateralTuning.torque.deadzone self.errors = [] self.tune = nTune(CP, self)
def __init__(self, CP, CI): super().__init__(CP, CI) self.scale = CP.lateralTuning.lqr.scale self.ki = CP.lateralTuning.lqr.ki self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) self.dc_gain = CP.lateralTuning.lqr.dcGain self.x_hat = np.array([[0], [0]]) self.i_unwind_rate = 0.3 * DT_CTRL self.i_rate = 1.0 * DT_CTRL self.reset() self.tune = nTune(CP, self)
def __init__(self, CP): self.LP = LanePlanner() self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost self.setup_mpc() self.solution_invalid_cnt = 0 self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False self.pre_auto_LCA_timer = 0.0 self.lane_change_BSM = LaneChangeBSM.off self.prev_torque_applied = False self.tune = nTune(CP)
def __init__(self, CP): self.scale = CP.lateralTuning.lqr.scale self.ki = CP.lateralTuning.lqr.ki self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) self.dc_gain = CP.lateralTuning.lqr.dcGain self.x_hat = np.array([[0], [0]]) self.i_unwind_rate = 0.3 * DT_CTRL self.i_rate = 1.0 * DT_CTRL self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.reset() self.tune = nTune(CP, self) # 추가