def main():
print "making a car"
car = Car()
lcd = car.lcd
buttons = (
(LCD.SELECT, "Select", (1, 1, 1)),
(LCD.LEFT, "Left", (1, 0, 0)),
(LCD.UP, "Up", (0, 0, 1)),
(LCD.DOWN, "Down", (0, 1, 0)),
(LCD.RIGHT, "Right", (1, 0, 1)),
)
while not shutdown_flag:
try:
x, y = car.front_position()
v = car.battery_voltage()
car.display_text("{:5.2f},{:5.2f}\n{:3.1f}v s rec >go".format(x, y, v))
if lcd.is_pressed(LCD.SELECT):
car.display_text("recording route\n-> stop")
make_recording(car=car)
car.display_text("recording\ncomplete")
time.sleep(1)
if lcd.is_pressed(LCD.RIGHT):
car.display_text("making route")
write_path_from_recording_file()
car.display_text("playing route")
play_route_main()
except IOError: # I2C error, keep trying
continue
except KeyboardInterrupt:
break
time.sleep(0.1)
car.display_text("goodbye")
del car
print ("goodbye")
time.sleep(1)
lcd.set_backlight(0)
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)
