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()