def reached_goal(goal, location, start):
(s_bearing, s_distance) = geomath.haversine(start.lat, start.lon, goal.lat,
goal.lon)
(c_bearing, c_distance) = geomath.haversine(location.lat, location.lon,
goal.lat, goal.lon)
distanceMeters = c_distance * 1000.0
close_enough = distanceMeters < WAYPOINT_DISTANCE_THRESHOLD
bearing_diff = (s_bearing - c_bearing) % 360.0
past_goal = 180 - BEARING_FLIP_THRESHOLD <= bearing_diff <= 180 + BEARING_FLIP_THRESHOLD
if past_goal:
print("PAST GOAL!")
if close_enough:
print("CLOSE ENOUGH!")
return past_goal or close_enough
def calculate_move(goal, location, start, drive_board, nav_board, speed):
(target_heading,
target_distance) = geomath.haversine(location.lat, location.lon, goal.lat,
goal.lon)
# The Haversine stuff works, reliably. Please let's use it instead.
# Crosstrack Correction as linear
#(xte_bearing, xte_dist) = geomath.crosstrack_error_vector(start, goal, location)
#goal_heading = geomath.weighted_average_angles([target_heading, xte_bearing], [1 - XTE_STRENGTH, XTE_STRENGTH])
"""
dx = goal.lon - location.lon
dy = goal.lat - location.lat
if dx / dy > 0:
goal_heading = math.degrees(math.atan(dx / dy))
if dy < 0:
goal_heading += 180
elif dy < 0:
goal_heading = 90 + math.degrees(math.atan(math.fabs(dy) / dx))
else:
goal_heading = 360 - math.degrees(math.atan(math.fabs(dx) / dy))
print("Curr Location: " + str(location) + ", Goal Heading: " + str(goal_heading))
c = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2)) * 10000
print(c)
speed = 150
if c < .9:
speed = int(speed * c) + 10
"""
#speed = 250 # increased for testing per Rausch
print(target_distance)
if target_distance < 0.01:
speed = 100
goal_heading = target_heading
print("Current heading: " + str(nav_board.heading()) + ", Goal:" +
str(goal_heading))
return hh.get_motor_power_from_heading(speed, goal_heading, drive_board,
nav_board)
elif state_switcher.state == rs.Idle():
#state_switcher.handle_event(rs.AutonomyEvents.START, rs.Idle())
pass
elif state_switcher.state == rs.ObstacleAvoidance():
rovecomm_node.write(drive.send_drive(0, 0))
# print(drive.send_drive(0,0))
# rovecomm_node.write(RoveCommPacket(1000, 'h', (0,0), ip_octet_4=140))
start = nav_board._location
rovecomm_node.write(drive.send_drive(-100,
-100)) # tune this drive number
# rovecomm_node.write(RoveCommPacket(1000, 'h', (-100,-100), ip_octet_4=140))
junk, distance = geomath.haversine(start[0], start[1],
nav_board._location[0],
nav_board._location[1])
while distance < 2:
if state_switcher.state != rs.ObstacleAvoidance():
rovecomm_node.write(drive.send_drive(0, 0))
continue
junk, distance = geomath.haversine(start[0], start[1],
nav_board._location[0],
nav_board._location[1])
print(distance)
time.sleep(0.1)
distance = 3
rovecomm_node.write(drive.send_drive(0, 0))
# rovecomm_node.write(RoveCommPacket(1000, 'h', (0,0), ip_octet_4=140))
r = 6371 # radius of earth
brng = math.radians(nav_board._heading - 90) # target heading