def enable_autonomy(packet_contents):
print(state_switcher.state)
if state_switcher.state == rs.Idle():
Logger.write_line(
time.strftime("%H%M%S") + " Enable: switching from Idle to Start")
print("Yo yo de o itsd a hard-no")
state_switcher.handle_event(rs.AutonomyEvents.START, rs.Idle())
set_gps_waypoint()
print("Throwing START")
elif state_switcher.state == rs.Shutdown():
Logger.write_line(
time.strftime("%H%M%S") +
" Enable: switching from Shutdown to Restart")
state_switcher.handle_event(rs.AutonomyEvents.RESTART, rs.Shutdown())
print("Throwing RESTART")
drive.enable()
def enable_autonomy(packet_contents):
print(state_switcher.state)
if state_switcher.state == rs.Idle():
# with open(logging, 'a') as f:
# f.write("Enable switching from Idle to Start\n")
print("Yo yo de o itsd a hard-no")
state_switcher.handle_event(rs.AutonomyEvents.START, rs.Idle())
set_gps_waypoint()
print("Throwing START")
elif state_switcher.state == rs.Shutdown():
# with open(logging, 'a') as f:
# f.write("Enable switching from Shutdown to Restart\n")
state_switcher.handle_event(rs.AutonomyEvents.RESTART,
rs.Shutdown(),
then=logging.info("Restarting Autonomy"))
print("Throwing RESTART")
drive.enable()
def enable_autonomy(packet_contents):
print(state_switcher.state)
if state_switcher.state == rs.Idle():
state_switcher.handle_event(rs.AutonomyEvents.START, then=logging.info("Enabling Autonomy"))
set_gps_waypoint()
print("Throwing START")
elif state_switcher.state == rs.Shutdown():
state_switcher.handle_event(rs.AutonomyEvents.RESTART, then=logging.info("Restarting Autonomy"))
print("Throwing RESTART")
drive.enable()
then=logging.info("Reached Marker"))
continue
else:
print("Driving To: " + str(left) + ", " + str(right))
rovecomm_node.write(drive.send_drive(left, right))
# rovecomm_node.write(RoveCommPacket(1000, 'h', (0,0), ip_octet_4=140))
else:
state_switcher.handle_event(rs.AutonomyEvents.MARKER_UNSEEN,
rs.ApproachingMarker())
print("Throwing MARKER_UNSEEN")
elif state_switcher.state == rs.Shutdown():
pass
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])
time.sleep(TIME_INTERVAL)
print("Rover reached a marker")
stateSwitcher.handle_event(rs.AutonomyEvents.REACHED_MARKER,
then=callback_func)
time.sleep(TIME_INTERVAL)
print("Test terminated")
counter = 0
while True:
if stateSwitcher.state == rs.Idle():
print("waiting")
counter += 1
if counter > 10:
stateSwitcher.handle_event(rs.AutonomyEvents.START)
elif stateSwitcher.state == rs.Navigating():
print("Rovin")
counter -= 2
if counter < 0:
stateSwitcher.handle_event(rs.AutonomyEvents.ABORT)
elif stateSwitcher.state == rs.Shutdown():
stateSwitcher.handle_event(rs.AutonomyEvents.RESTART)