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