def test_plot_ballistic_velocity_profile(self):
aim_short_dist = 0.0
max_vel = 1.0
max_accel = 0.5
dt = 0.1
profile = VelocityProfiler( aim_short_dist, max_vel, max_accel, dt)
loop_num = 0
distance_to_travel = 10.0
distance_left = distance_to_travel
distance_traveled = 0.0
finished = False
loop_arr = [0]
dist_left_arr = [distance_left]
dist_traveled_arr = [0]
vel_arr = [0]
while not finished:
(velocity, finished, goal_reached) = \
profile.next_profile_step_ballistic(distance_left, loop_num)
dist = (velocity * dt)
distance_traveled += dist
distance_left -= dist
loop_num += 1
loop_arr.append(loop_num)
vel_arr.append(velocity)
dist_left_arr.append(distance_left)
dist_traveled_arr.append(distance_traveled)
abs(distance_to_travel)
abs(distance_traveled)
max_dist = distance_to_travel + (max_accel * dt)
min_dist = distance_to_travel - (max_accel * dt)
print 'Ballistic Actual Distance %g, Expected %g' %
(distance_traveled, distance_to_travel)
def run_ballistic_velocity_profile(distance_to_travel, max_vel, max_accel, dt,
aim_short_dist):
''' Runs and plots a ballisitic (open-loop) velocity profile. '''
run_stats = 'Distance: %g Max Velocity: %g Max Acceleration: %g dt: %g Aim Short:%g '\
% (distance_to_travel, max_vel, max_accel, dt, aim_short_dist)
print '+++ Ballistic Profile +++'
print run_stats
profile = VelocityProfiler( aim_short_dist, max_vel, max_accel, dt)
loop_num = 0
distance_left = distance_to_travel
distance_traveled = 0.0
finished = False
loop_arr = [0]
dist_left_arr = [distance_left]
dist_traveled_arr = [0]
vel_arr = [0]
error_loops = []
error_vels = []
while not finished:
(velocity, finished, goal_reached) = \
profile.next_profile_step_ballistic(distance_left, loop_num)
dist = (velocity * dt)
distance_traveled += dist
distance_left -= dist
# whine but don't fail if we exceed accel limits
loop_num += 1
accel = abs(velocity - vel_arr[loop_num - 1]) / dt
if ( accel > max_accel + 0.000001): # addition for floating point alaising
print 'Loop %g acceleration %g is too high (max %g), velocity %g' % \
(loop_num, accel, max_accel, velocity)
error_loops.append(loop_num - 1)
error_loops.append(loop_num)
error_vels.append(vel_arr[loop_num - 1])
error_vels.append(velocity)
# Store all the datapoints to display
loop_arr.append(loop_num)
vel_arr.append(velocity)
# dist_left_arr.append(distance_left)
dist_traveled_arr.append(distance_traveled)
# clear last data point for pretty graphs ;)
loop_arr.pop()
vel_arr.pop()
dist_traveled_arr.pop()
title = 'Ballistic\n%s\nDistance to Target: %f' % (run_stats, distance_left)
generate_plots( loop_arr, dist_traveled_arr, vel_arr, loop_num,
distance_to_travel, max_vel, error_loops, error_vels,
title)
print '+++ Ballistic Actual Distance %g, Expected %g +++\n' % \
(distance_traveled, distance_to_travel)
'''
