def go():
f = FileNames()
config.run_name = f.next_run_name(config.track_name, config.route_name)
run_folder = f.get_run_folder(config.track_name, config.route_name, config.run_name)
os.makedirs(run_folder)
dump_config(f.config_file_path(config.track_name, config.route_name, config.run_name))
car.reset_odometry()
input_path = f.path_file_path(config.track_name,config.route_name)
stereo_video_paths = f.stereo_video_file_paths(config.track_name,config.route_name,config.run_name)
if config.capture_video:
capture = vision.capture.Capture(stereo_video_paths=stereo_video_paths)
capture.begin()
rte = route.Route()
car.display_text('loading route')
rte.load_from_file(input_path)
rte.smooth(k_smooth=config.k_smooth)
rte.optimize_velocity(max_velocity = config.max_v, max_acceleration = config.max_a)
car.display_text('playing route')
recording_file_path = FileNames().recording_file_path(config.track_name,config.route_name,config.run_name)
car.begin_recording_input(recording_file_path)
car.begin_recording_commands(FileNames().commands_recording_file_path(config.track_name,config.route_name,config.run_name))
try:
play_route(rte, car, k_smooth = config.k_smooth, d_ahead = config.d_ahead, t_ahead = config.t_ahead)
except:
path = f.exception_file_path(config.track_name,config.route_name,config.run_name)
with open(path,'w') as log:
log.write(exception.exception_text())
finally:
car.end_recording_input()
car.end_recording_commands()
if config.capture_video:
capture.end()
car.lcd.display_text("making path")
path_file_path = f.path_file_path(config.track_name,config.route_name,config.run_name)
write_path_from_recording_file(inpath=recording_file_path,outpath=path_file_path)
#!/usr/bin/env python
# from car import Car
from play_route import play_route
from route import Route, straight_route
# car = Car()
import argparse
parser = argparse.ArgumentParser(description="Route follower")
parser.add_argument("--distance", nargs="?", type=float, default=3.0, help="distance")
parser.add_argument("--max_a", nargs="?", type=float, default=1.0, help="max_acceleration")
parser.add_argument("--max_v", nargs="?", type=float, default="1.0", help="max_velocity")
args = parser.parse_args()
max_a = args.max_a
max_v = args.max_v
distance = args.distance
route = forward_back_route(distance, max_a, max_v)
play_route(route)
# car.forward(meters, max_speed = max_speed)
# car.forward(-meters, max_speed = max_speed)
def calibrate_braking(test_esc=1350,max_speed=1.,total_track_length=3.,stop_track_length=2.,note='na'):
run_track_length = total_track_length - stop_track_length
car = Car()
time.sleep(0.5)
car.zero_odometer()
car.set_rc_mode();
goal_heading = car.heading_degrees()
# accelerate in straight line until you get to desired speed
print ('accelerating to speed of '+str(max_speed))
v = 0.0 # assume we start at zero speed
aborted = False
while v < max_speed:
if car.odometer_meters() > run_track_length:
aborted = True
abort_reason = "ran out of track"
break;
esc = car.esc_for_velocity(v+2.5)
steer = car.steering_for_goal_heading_degrees(goal_heading)
car.set_esc_and_str(esc,steer)
time.sleep(0.02)
v = car.get_velocity_meters_per_second()
print('done accelerating at {:4.1f} meters'.format(car.odometer_meters()))
print('testing at esc: {}'.format(test_esc))
queue = Queue.Queue()
car.add_listener(queue)
data = []
# set the esc to given test setting
while True:
# go straight and record data until one of three things happens
esc = test_esc
steer = car.steering_for_goal_heading_degrees(goal_heading)
car.set_esc_and_str(esc,steer)
message = queue.get(block=True, timeout = 0.05)
data.append(message)
#
# 1. You come to a full stop
v = car.get_velocity_meters_per_second()
if v <= 0.:
print('test concluded at velocity zero')
break
# 2. Your speed stabilizes
# 3. You reach distance limit
if car.odometer_meters() > run_track_length:
print('test length exceeded, allowing room to stop')
break
# 4. You detect that you are skidding
car.remove_listener(queue)
if v > 0:
print('applying safety brake at {} meters and velocity {}'.format(car.odometer_meters(), v))
# put on "safe level" of brakes until your speed reaches zero (or negative)
while v > 0:
esc = 1350
steer = car.steering_for_goal_heading_degrees(goal_heading)
car.set_esc_and_str(esc,steer)
time.sleep(0.02)
v = car.get_velocity_meters_per_second()
# set everything to neutral
final_distance = car.odometer_meters()
print('final distance: {} meters'.format(final_distance))
car.set_manual_mode()
# save the results of braking to two files
# car dynamics in one file
# test settings in another file
# do a quick analysis of data and print results
prefix = 'esc/{}_start_{}_esc_{}'.format(note,int(max_speed*10),int(test_esc))
f = open(next_filename(folder = 'data', prefix = prefix, suffix = '.csv'), 'w')
print ('seconds,meters,us,esc,odometer_ticks,odometer_last_us,ax,spur_delta_us,spur_odo',file=f)
for p in data:
p.seconds = (p.us-data[0].us)/1000000.
p.meters = (p.odometer_ticks-data[0].odometer_ticks)*car.meters_per_odometer_tick
fields = [p.seconds,p.meters,p.us,p.esc,p.odometer_ticks,p.odometer_last_us,p.ax,p.spur_delta_us,p.spur_odo]
print(",".join([str(field) for field in fields]), file=f)
print('np polyfit 2 result',np.polyfit([p.seconds for p in data], [p.meters for p in data], 2))
print('np polyfit 3 result',np.polyfit([p.seconds for p in data], [p.meters for p in data], 3))
# distance to stop from speed, distance left in track
# whether skidding occurred
# estimate of acceleration using linear fit
# estimate of acceleration using polynomial fit (TBD: model for this, probably adding k*v^2 term for wind resistance)
car.reset_odometry()
time.sleep(0.01)
route = reverse_route(final_distance, max_a=0.5, max_v=2.)
play_route(route, car = car)
car.set_manual_mode()
