import algorithms.geomath as geomath
import algorithms.followBall as follow_ball
#logging = "./logs/" + time.strftime("%Y%m%d-%H%M%S") + ".txt"
#loggingFile = open(logging,'w')
#loggingFile.write("Beginning Logging at time in file name\n")
#loggingFile.close()
#print(logging)
# Hardware Setup
rovecomm_node = RoveCommEthernetUdp("hi")
drive = DriveBoard("hi")
nav_board = NavBoard(rovecomm_node, "hi")
state_switcher = rs.StateSwitcher("hi")
waypoints = deque()
gps_data = GPSData()
tracker = ObjectTracker()
print("Setup complete")
# Assign callbacks for incoming messages
def add_waypoint_handler(packet_contents):
latitude, longitude = packet_contents.data
waypoint = constants.Coordinate(latitude, longitude)
waypoints.append(waypoint)
print("YO im addin a point")
#loggingFile = open(logging, 'a')
#log = "Add waypoint lat,lon: " + str(latitude) + "," + str(longitude) + "\n"
import logging
import time
import constants
import rover_states
import algorithms.marker_search as marker_search
import algorithms.gps_navigate as gps_nav
state_switcher = rover_states.StateSwitcher()
state_switcher.state = rover_states.Searching()
gps_data = gps_nav.GPSData()
gps_data.start = constants.Coordinate(37.950271, -91.777770)
current_loc = gps_data.start
gps_data.goal = marker_search.calculate_next_coordinate(
gps_data.start, current_loc)
print("Origin: " + str(gps_data.start.lat) + ", " + str(gps_data.start.lon))
while True:
if state_switcher.state == rover_states.Searching():
print()
goal, start = gps_data.data()
#print("Current: " + str(current_loc))
#print("Goal: " + str(goal))
new_goal = marker_search.calculate_next_coordinate(start, current_loc)
print("New Goal: " + str(new_goal.lat) + ", " + str(new_goal.lon))
current_loc = goal
import rover_states as rs
import time
def callback_func():
print("Running callback")
time.sleep(TIME_INTERVAL)
print("Switched to state: " + str(stateSwitcher.state) + "\n")
stateSwitcher = rs.StateSwitcher()
TIME_INTERVAL = 2
time.sleep(TIME_INTERVAL)
print("Starting autonomy...")
stateSwitcher.handle_event(rs.AutonomyEvents.START)
print("Switched to state: " + str(stateSwitcher.state) + "\n")
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
outString = "./logs/" + time.strftime("%Y%m%d-%H%M%S") + ".txt"
outFile = open(outString, 'w')
outFile.write(time.strftime("%Y%m%d-%H%M%S") + "\n")
outFile.close()
print(outString)
# Hardware Setup
rovecomm_node = RoveCommEthernetUdp(outString)
print("RoveComm")
drive = DriveBoard(outString)
print("DriveBoard")
nav_board = NavBoard(rovecomm_node, outString)
notify = Notify(rovecomm_node)
print("NavBoard")
state_switcher = rs.StateSwitcher(outString)
print("StateSwitcher")
waypoints = deque()
print("waypoint")
gps_data = GPSData()
print("GPSData")
tracker = ObjectTracker()
print("Cameras")
loopDelay = 0.07
print("Setup complete")
# Assign callbacks for incoming messages
def add_waypoint_handler(packet_contents):
latitude, longitude = packet_contents.data
waypoint = constants.Coordinate(latitude, longitude)