def dropnow(wind_x, wind_y, drop_pt, timestamp, last_dropped, drop_pkg,
velocity_adjusted):
# if vehicle velocity vector and vector to drop point are roughly collinear
# then see how close the drop point is and drop accordingly
# don't need to be "exaclty" collinear because of floating point math and
# wind affect on the package as it falls
# check for collinearity by comparing ratio of x and y components (should be exactly
# equal if vectors are exactly collinear)
# print("vehicle velocity: ", vehicle_velocity)
# print("drop x, y: ", drop_x, drop_y)
#if (abs(vehicle_velocity[0]/float(drop_x) - vehicle_velocity[1]/float(drop_y)) < 0.3):
drop_x, drop_y = pm.convert_to_cartesian(drop_pt[0], drop_pt[1])
vehicle_velocity = pm.convert_to_polar(velocity_adjusted[0],
velocity_adjusted[1])
speed = vehicle_velocity[0] #* math.cos(math.radians(vehicle_velocity[1]))
# using horizontal launch and free fall equations
# ignoring whether the wind will affect the trajectory on the way down
# Range = speed * time of flight and time of flight is defined as 0.5
fallrange = speed * 0.5
if (abs(fallrange - pm.distance(drop_x, drop_y)) < 1.5):
# edge case is that when there is a tree on the very edge, package is
# being dropped
# either this shouldn't be put in "maybe_drop point" or need to test
# for that situation here
# we don't want to drop 2 packages in same drop area, I figure that
# if the vehicle is moving forward 30m/s and the drop zone is 10m
# diameter, then 1 second later the vehicle wouldn't be in the same
# drop zone. Obviously massive headwind and lateral motion back and
# forth could make this assumption incorrect
if (timestamp - last_dropped > 1000):
drop_pkg = 1
return drop_pkg
def go_where(timestamp, recovery_x, wind_x, wind_y, recovery_y, lidar_samples,
last_dropped, drop_pkg, prior_trees, my_velocity,
velocity_adjusted):
# if recovery_distance is close, GO THERE EVEN IF ALL PACKAGES NOT DROPPED
# if already dropped 10 packages, head to recovery center
# avoid trees
# find where the trees are and where delivery sites are that
# drops and trees are lists of tuples with distance and angle
# these are only deconflicted drops that don't have a tree in the way
drops, trees = checklidar(lidar_samples, prior_trees)
# if the recovery distance is close, then head there, avoiding trees
recovery_dist = pm.distance(recovery_x, recovery_y)
if (recovery_dist < 250):
if (trees):
desired_y = avoid_tree(wind_x, wind_y, trees)
else:
mag, theta = pm.convert_to_polar(recovery_x, recovery_y)
desired_y = pm.target_velocity(wind_x, wind_y, theta, my_velocity)
# find the closest drop point if there are drop points:
elif drops:
drop_pt = min(drops)
print("angle to drop point: ", drop_pt[1])
# the returned drop point will be drop point without a tree in the way
# if the coast is clear to go to this point:
desired_y = target_velocity(wind_x, wind_y, drop_pt[1], my_velocity)
# decide whether the drop point is close enough to command a drop
drop_pkg = dropnow(wind_x, wind_y, drop_pt, timestamp, last_dropped,
drop_pkg, velocity_adjusted)
# if there is no course for a drop point without a tree in the way, check
# to see if a tree should be avoided
else:
if (trees):
desired_y = avoid_tree(wind_x, wind_y, trees, my_velocity)
else:
# if there are no trees, then just stay the straight course ie have
# desired y be the opposite of the wind vector_y component
desired_y = -wind_y
# desired_y can only be in the parameters of what the game allows
if (desired_y > 30):
desired_y = 30
elif (desired_y < -30):
desired_y = -30
return desired_y, drop_pkg, trees
round(RECOVERY_X - vehicle.position[0]), wind.vector[0],
wind.vector[1],
round((-vehicle.position[1] + WORLD_WIDTH_HALF) %
WORLD_WIDTH - WORLD_WIDTH_HALF), *lidar_samples)
print("telem struct to send: \n")
print(structure1)
print(struct.unpack(">Hhffb31B", structure1))
timestamp, recovery_x_error, wind_x, wind_y, recovery_y_error, lidar_samples = pm.parse_telem(
structure1)
print("lidar samples: ", lidar_samples)
print("wind x: ", wind_x, " wind y: ", wind_y)
# this is the lateral and forward velocity that I am controlling
a_vel = pm.current_velocity(wind_x, wind_y, my_velocity)
a_vel_polar = pm.convert_to_polar(a_vel[0], a_vel[1])
print("my_velocity: ", my_velocity)
print("actual velocity: ", a_vel, " polar: ", a_vel_polar)
droplist, treelist = checklidar(lidar_samples, prior_trees)
print("droplist: ", droplist)
print("treelist: ", treelist)
prior_trees = treelist
if (treelist):
desired_y = avoid_tree(wind_x, wind_y, treelist,
my_velocity)
my_velocity[1] = desired_y
if droplist:
drop_pt = min(droplist)
print("drop point: ", drop_pt)