Python Car.get_velocity_meters_per_second方法代码示例

# 需要导入模块: from car import Car [as 别名]
# 或者: from car.Car import get_velocity_meters_per_second [as 别名]
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)

# 需要导入模块: from car import Car [as 别名]
# 或者: from car.Car import get_velocity_meters_per_second [as 别名]
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()