def playback(): record_start_time = None playback_start_time = datetime.datetime.now() car = Car() car.set_rc_mode() start_heading = car.dynamics.heading recording = Recording() while recording.read(): dyn = recording.current() if record_start_time == None: record_start_time = dyn.datetime record_start_heading = dyn.heading t_now = datetime.datetime.now() t_wait = (dyn.datetime - record_start_time) - (t_now - playback_start_time) if t_wait.total_seconds() > 0: time.sleep(t_wait.total_seconds()) # adjust steering based on heading error actual_turn = degrees_diff(start_heading, car.dynamics.heading) expected_turn = degrees_diff(record_start_heading, dyn.heading) original_steer_angle = car.angle_for_steering(dyn.str) steer_angle = original_steer_angle + degrees_diff(actual_turn, expected_turn) str = car.steering_for_angle(steer_angle) car.set_esc_and_str(dyn.esc, str) car.set_manual_mode() print 'all done'
import time from car import Car car = Car() try: car.set_rc_mode() steer_angle = 0.0 max_angle = 30.0 min_angle = -max_angle step_seconds = 0.02 step = max_degrees_per_second * step_seconds while True: steer_angle += step if steer_angle >= max_angle: step = -abs(step) if steer_angle <= min_angle: step = abs(step) steer_ms = car.steering_for_angle(steer_angle) car.set_esc_and_str(1500, steer_ms) time.sleep(0.02) finally: car.set_manual_mode()
import time from car import Car from geometry import * #goal is to find a way to point the wheels straight ahead car = Car() time.sleep(1) original_heading = car.dynamics.heading car.set_rc_mode() speed = 1500 while(True): heading = angle_diff(original_heading, car.dynamics.heading) steering = car.steering_for_angle(-heading) car.set_esc_and_str(speed, steering) #print "Heading: {} Steering: {}".format(heading, steering) time.sleep(0.01) car.set_manual_mode()
def play_route(route, car = None, print_progress = False, k_smooth = 0.4, d_ahead = 0.05, t_ahead = 0.2): last_ms = None #print route if car is None: car = Car() queue = Queue.Queue() try: car.set_rc_mode() car.add_listener(queue) message = queue.get(block=True, timeout = 0.5) #print repr(message) last_ms = message.ms start_time = time.time() # keep going until we run out of track car.lcd.display_text('press any key\nto abort') while car.lcd.getch() is None: try: message = queue.get(block=True, timeout = 0.5) except: print 'message timed out at: '+datetime.datetime.now().strftime("%H:%M:%S.%f") print 'last message received:' + repr(message) print raise (x,y) = car.front_position() (rear_x,rear_y) = car.rear_position() car_velocity = car.get_velocity_meters_per_second() route.set_position(x,y,rear_x,rear_y,car_velocity) steering_angle = steering_angle_by_look_ahead_curve(car,route,d_ahead,t_ahead) #steering_angle = steering_angle_by_look_ahead(car,route,d_ahead,t_ahead) str_ms = car.steering_for_angle(steering_angle) esc_ms = esc_for_velocity(route.velocity(), car, route.is_reverse()) if print_progress: print("t: {:.1f} i: {} xg: {:.2f} gy:{:.2f} gv: {:.2f} v:{:.2f} x: {:.2f} y:{:.2f} reverse: {} cte:{:.2f} heading:{:.2f} segment_heading: {:.2f} steering_degrees: {:.2f} esc:{}".format( time.time() - start_time, route.index, route.nodes[route.index+1].x, route.nodes[route.index+1].y, route.velocity(), car_velocity, x, y, route.is_reverse(), cte, car_heading, segment_heading, steering_angle, esc_ms)) # send to car car.set_esc_and_str(esc_ms, str_ms) if route.done() and car_velocity == 0: break; finally: car.set_esc_and_str(1500,1500) car.set_manual_mode() car.remove_listener(queue)
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()