|
|
|
@ -13,7 +13,7 @@ def calc_d_lookahead(v_ego): |
|
|
|
|
# howfar we look ahead is function of speed |
|
|
|
|
offset_lookahead = 1. |
|
|
|
|
coeff_lookahead = 4.4 |
|
|
|
|
# sqrt on speed is needed to keep, for a given curvature, the y_offset |
|
|
|
|
# sqrt on speed is needed to keep, for a given curvature, the y_offset |
|
|
|
|
# proportional to speed. Indeed, y_offset is prop to d_lookahead^2 |
|
|
|
|
# 26m at 25m/s |
|
|
|
|
d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * coeff_lookahead |
|
|
|
@ -31,7 +31,7 @@ def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max, |
|
|
|
|
steer_override, sat_count, enabled, Kp, Ki, rate): |
|
|
|
|
|
|
|
|
|
sat_count_rate = 1./rate |
|
|
|
|
sat_count_limit = 1.2 # after 0.8s of continuous saturation, an alert will be sent |
|
|
|
|
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent |
|
|
|
|
|
|
|
|
|
error_steer = y_des - y_actual |
|
|
|
|
Ui_unwind_speed = 0.3/rate #.3 per second |
|
|
|
|