Skip to content

Commit

Permalink
Added initial control version from @Ridhwanluthra @gurgentus and @jus…
Browse files Browse the repository at this point in the history 
…tanotherdevel
  • Loading branch information
@ingenieroariel
ingenieroariel committed Feb 6, 2017
1 parent cd62e75 commit a4c9ad3
Show file tree
Hide file tree
Showing 6 changed files with 403 additions and 116 deletions.
65 changes: 56 additions & 9 deletions acc/acc/cruise.py
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,57 @@

def control(speed, acceleration, car_in_front, steer_torque):
"""Adaptive Cruise Control
"""
# --- Implement your solution here ---#
brake = 0
gas = 0
# ------------------------------------#

return brake, gas

def control(speed=0, acceleration=0, car_in_front=200, gap=5, cruise_speed=None, state=None):
"""Adaptive Cruise Control
speed: Current car speed (m/s)
acceleration: Current car acceleration (m/s^2)
gas: last signal sent. Real number.
brake: last signal sent. Real number.
car_in_front: distance in meters to the car in front. (m)
gap: maximum distance to the car in front (m)
cruise_speed: desired speed, set via the cruise control buttons. (m/s)
status: a convenience dictionary to keep state between runs.
"""

if state is None:
state = dict(K_p=0.15, K_d=0., K_i=0.0003, d_front_prev=100,
t_safe=.5, prev_setpoint=0., integral_setpoint=0.,
maintaining_distance=False)

delta_distance = car_in_front - 2 * gap - speed**2 / (2 * 5)

# if the car ahead does not allow to get to cruise speed
# use safe following distance as a measure until cruise speed is reached again
if delta_distance < 0:
state['maintaining_distance'] = True
elif speed >= cruise_speed:
state['maintaining_distance'] = False

if state['maintaining_distance']:
# But override it if we are too close to the car in front.
set_point = delta_distance
else:
# if the distance is not too close maintain cruise speed
set_point = cruise_speed - speed

control = (state['K_p'] * set_point +
state['K_d'] * (set_point - state['prev_setpoint']) +
state['K_i'] * state['integral_setpoint'])

if control > 1:
control = 1.
elif control < -1:
control = -1.

if control >= 0:
gas = control
brake = 0
if control < 0:
gas = 0
brake = -control

# ------set variables from previous value-----
state['prev_setpoint'] = set_point
state['integral_setpoint'] = state['integral_setpoint'] + set_point

return brake, gas, state
0 acc/LICENSE.tests → acc/acc/tests/LICENSE.tests
View file
File renamed without changes.
117 changes: 98 additions & 19 deletions acc/acc/tests/maneuver.py
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,19 @@
from .plant import Plant
from .visualize import Visualizer
import numpy as np


class CV:
MPH_TO_MS = 1.609 / 3.6
MS_TO_MPH = 3.6 / 1.609
KPH_TO_MS = 1. / 3.6
MS_TO_KPH = 3.6
MPH_TO_KPH = 1.609
KPH_TO_MPH = 1. / 1.609
KNOTS_TO_MS = 1 / 1.9438
MS_TO_KNOTS = 1.9438


class Maneuver(object):

def __init__(self, title, duration, **kwargs):
Expand All @@ -17,12 +29,12 @@ def __init__(self, title, duration, **kwargs):
self.speed_lead_breakpoints = kwargs.get(
"speed_lead_values", [0.0, duration])

self.cruise_button_presses = kwargs.get("cruise_button_presses", [])
self.cruise_speeds = kwargs.get("cruise_speeds", [])

self.duration = duration
self.title = title

def evaluate(self, control=None, verbosity=0, min_gap=5):
def evaluate(self, control=None, verbosity=0, gap=10, plot=False, animate=False):
"""runs the plant sim and returns (score, run_data)"""
plant = Plant(
lead_relevancy=self.lead_relevancy,
Expand All @@ -31,19 +43,40 @@ def evaluate(self, control=None, verbosity=0, min_gap=5):
verbosity=verbosity,
)

buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
current_button = 0
speeds_sorted = sorted(self.cruise_speeds, key=lambda a: a[1])
cruise_speed = 0

brake = 0
gas = 0
steer_torque = 0

# dictionary used by the cruise control function to maintan state between invocations.
state = None

# 3 possible states(accelerating(1), not accelerating(0), braking(-1))
previous_state = 0
score = 0.
prev_accel = 0.
# TODO: calibrate this threshold to denote maximum discomfort allowed
score_threshold = 200.
# TODO: calibrate this constant for scaling rate of acceleration
accel_const = 1.

# Initialize the Visualizer. Set animate = False to only display the plots at the end of the maneuver,
# this will be faster than showing in real time with animate = True
# max_speed, max_accel, max_score set the maximum for the y-axis
# TODO: make this dynamic?

if verbosity >= 4:
vis = Visualizer(animate=animate, show=plot, max_speed=80, max_accel=10, max_score=20)

while plant.current_time() < self.duration:
while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
current_button = buttons_sorted[0][0]
buttons_sorted = buttons_sorted[1:]
while speeds_sorted and plant.current_time() >= speeds_sorted[0][1]:
# getting the current cruise speed
cruise_speed = speeds_sorted[0][0]
speeds_sorted = speeds_sorted[1:]
if verbosity > 1:
print("current button changed to", current_button)
print("current cruise speed changed to", cruise_speed)

grade = np.interp(plant.current_time(),
self.grade_breakpoints, self.grade_values)
Expand All @@ -53,18 +86,64 @@ def evaluate(self, control=None, verbosity=0, min_gap=5):
speed, acceleration, car_in_front, steer_torque = plant.step(brake=brake,
gas=gas,
v_lead=speed_lead,
cruise_buttons=current_button,
grade=grade)

# If the car in front is less than min_gap away, give it the worst score
# and abort.
if car_in_front < min_gap:
return 0
# Assert the gap parameter is respected during all the maneuver.
assert car_in_front >= gap

brake, gas, state = control(speed=speed,
acceleration=acceleration,
car_in_front=car_in_front,
gap=gap,
cruise_speed=cruise_speed)

if gas > 0:
# accelerating
new_state = 1
elif brake > 0:
# braking
new_state = -1
else:
# not accelerating
new_state = 0

# getting the rate of change of acceleration
# TODO: add division by exact time, if relevent(did not delve deep into timekeeping)
rate_accel = acceleration - prev_accel
prev_accel = acceleration

# based on acceptable jerk values given in
# A SURVEY OF LONGITUDINAL ACCELERATION COMFORT STUDIES
# IN GROUND TRANSPORTATION VEHICLES by l. l. HOBEROCK
# not sure about the validity, real tests should proove this
# uncomment this with the acceptable values, this will affect pid consts
# assert -0.3 * 9.81 < rate_accel < 0.3 * 9.81

# The higher the value of score, worse the controller.
# multiplication with rate_accel scales the change based on the speed of change.
score += abs((new_state - previous_state) + rate_accel * accel_const)
previous_state = new_state

# this updates the plots with latest state

if verbosity >= 4:
vis.update_data(cur_time=plant.current_time(), speed=speed,
acceleration=acceleration, gas_control=gas, brake_control=brake,
car_in_front=car_in_front, steer_torque=steer_torque, score=score)

score /= self.duration
assert score <= score_threshold

# Assert the desired speed matches the actual speed at the end of the maneuver.
# only valid when lead distance is greater than distance to be maintained
if car_in_front >= 200:
# TODO: Make atol lower once we have a decent solution.
assert np.isclose(speed, cruise_speed, atol=2)

# TODO: if there is a car in front, assert the speed matches that car's speed at the end of the manuever.

brake, gas = control(speed, acceleration,
car_in_front, steer_torque)
# this cleans up the plots for this maneuver and pauses until user presses [Enter]
if verbosity >= 4 and plot:
vis.show_final_plots()

# TODO: Calculate score, for now it always returns 10.
# It should be 0 when the car crashes and higher if it doesn't.
score = 10
return score
return
70 changes: 35 additions & 35 deletions acc/acc/tests/plant.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,14 +49,16 @@ def monitor_time(self):

def car_plant(pos, speed, grade, gas, brake):
# vehicle parameters
g = 9.81
mass = 1700
aero_cd = 0.3
force_peak = mass * 3.
force_brake_peak = -mass * 10. # 1g
# force_peak is set to get maximum acceleration allowed of 0.4g
force_peak = mass * 1.24
# force_brake_peak is set to get maximum decceleration allowed -0.9g
force_brake_peak = -mass * 43
power_peak = 100000 # 100kW
speed_base = power_peak / force_peak
rolling_res = 0.01
g = 9.81
air_density = 1.225
gas_to_peak_linear_slope = 3.33
brake_to_peak_linear_slope = 0.2
Expand Down Expand Up @@ -98,7 +100,6 @@ def __init__(self, lead_relevancy=False, rate=100,

self.angle_steer = 0.
self.gear_choice = 0
self.speed, self.speed_prev = 0., 0.

self.esp_disabled = 0
self.main_on = 1
Expand All @@ -107,6 +108,7 @@ def __init__(self, lead_relevancy=False, rate=100,
self.brake_pressed = 0
self.distance, self.distance_prev = 0., 0.
self.speed, self.speed_prev = speed, speed
self.acceleration_prev = None
self.steer_error, self.brake_error, self.steer_not_allowed = 0, 0, 0
self.gear_shifter = 4 # D gear
self.pedal_gas = 0
Expand All @@ -119,21 +121,15 @@ def __init__(self, lead_relevancy=False, rate=100,
self.lead_relevancy = lead_relevancy

# lead car
self.distance_lead, self.distance_lead_prev = distance_lead, distance_lead
self.distance_lead_prev = distance_lead

self.rk = Ratekeeper(rate, print_delay_threshold=100)
self.ts = 1. / rate

def speed_sensor(self, speed):
if speed < 0.3:
return 0
else:
return speed

def current_time(self):
return float(self.rk.frame) / self.rate

def step(self, brake=0, gas=0, steer_torque=0, v_lead=0.0, cruise_buttons=None, grade=0.0):
def step(self, brake=0, gas=0, steer_torque=0, v_lead=0.0, grade=0.0):
# dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)

distance_lead = self.distance_lead_prev + v_lead * self.ts
Expand All @@ -152,49 +148,53 @@ def step(self, brake=0, gas=0, steer_torque=0, v_lead=0.0, cruise_buttons=None,

# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., self.distance_lead - distance)
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
else:
d_rel = 200.
v_rel = 0.

# set sensible value to acceleration for first iteration
if self.acceleration_prev is None:
self.acceleration_prev = acceleration
# print at 5hz
if (self.rk.frame % (self.rate / 5)) == 0:
msg_tmpl = ("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang "
# if (self.rk.frame % (self.rate / 5)) == 0:
if True:
msg_tmpl = ("%6.2f m %6.2f m/s %6.2f m/s2 %.2f m/s3 "
" gas: %.2f brake: %.2f steer: %5.2f "
" lead_rel: %6.2f m %6.2f m/s")
msg = msg_tmpl % (distance, speed, acceleration, self.angle_steer,
msg = msg_tmpl % (distance, speed, acceleration, acceleration - self.acceleration_prev,
gas, brake, steer_torque, d_rel, v_rel)

if self.verbosity > 2:
print(msg)

# ******** publish the car ********
vls = [self.speed_sensor(speed), self.speed_sensor(speed),
self.speed_sensor(speed), self.speed_sensor(speed),
self.angle_steer, 0, self.gear_choice, speed != 0,
0, 0, 0, 0,
self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed,
self.user_gas, cruise_buttons, self.esp_disabled, 0,
self.user_brake, self.steer_error, self.speed_sensor(
speed), self.brake_error,
self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
self.pedal_gas, self.cruise_setting,
# left_blinker, right_blinker, counter
0, 0, 0,
# interceptor_gas
0, 0]

# TODO: Use vls for something
assert vls is not None
# vls = [self.speed_sensor(speed), self.speed_sensor(speed),
# self.speed_sensor(speed), self.speed_sensor(speed),
# self.angle_steer, 0, self.gear_choice, speed != 0,
# 0, 0, 0, 0,
# self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed,
# self.user_gas, cruise_buttons, self.esp_disabled, 0,
# self.user_brake, self.steer_error, self.speed_sensor(
# speed), self.brake_error,
# self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
# self.pedal_gas, self.cruise_setting,
# # left_blinker, right_blinker, counter
# 0, 0, 0,
# # interceptor_gas
# 0, 0]

# # TODO: Use vls for something
# assert vls is not None

# ******** update prevs ********
self.speed_prev = speed
self.distance_prev = distance
self.distance_lead_prev = distance_lead
self.acceleration_prev = acceleration

car_in_front = distance_lead - \
distance if self.lead_relevancy else 200.
car_in_front = distance_lead - distance if self.lead_relevancy else 200.

self.rk.keep_time()
return (speed, acceleration, car_in_front, steer_torque)
Loading

0 comments on commit a4c9ad3

Please sign in to comment.