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
def target_velocity(wind_x, wind_y, desired_angle, my_velocity):
# given the current wind speed, the desired angle of travel,
# and the current velocity (global)
# compute the y component of the new desired velocity vector
# note: the x component never changes
# vel_y = proportionality constant * (magnitude to travel/size timestep)
# * sin(desired_angle) - (wind vector dot velocity)/(magnitude velocity)
# set magnitude to travel to 1m since with no wind and no lateral airspeed,
# would be traveling 0.5 m/timestep
timestep = 1 / 60 # size of timestep in seconds
magnitude_v = pm.distance(my_velocity[0], my_velocity[1]) * timestep
p = 20 # experimentally determined proportionality constant
vel_y = ((1 * p / timestep) * math.sin(math.radians(desired_angle)) - (
(wind_x * my_velocity[0] + wind_y * my_velocity[1]) / magnitude_v))
return vel_y