diff --git a/build/lib/libs/Drive.py b/build/lib/libs/Drive.py new file mode 100644 index 0000000..f884232 --- /dev/null +++ b/build/lib/libs/Drive.py @@ -0,0 +1,313 @@ +from threading import Thread +from threading import Timer +import configparser +import os +os.chdir(os.path.dirname(os.path.abspath(__file__))) +import math +from time import sleep +from nis import maps + +import sys +sys.path.append('../../Mission Control/RoverMap/') +from server import MapServer + +from libs import UDPOut +from libs import Location +from libs import ARTracker + +class Drive: + + def __init__(self, baseSpeed, cameras): + self.baseSpeed = baseSpeed + self.tracker = ARTracker.ARTracker(cameras) + + #Starts everything needed by the map + self.mapServer = MapServer() + self.mapServer.register_routes() + self.mapServer.start(debug=False) + self.startMap(self.updateMap, .5) + sleep(.1) + + #sets up the parser + config = configparser.ConfigParser(allow_no_value=True) + config.read(os.path.dirname(__file__) + '/../config.ini') + + #parses config + self.mbedIP = str(config['CONFIG']['MBED_IP']) + self.mbedPort = int(config['CONFIG']['MBED_PORT']) + swiftIP = str(config['CONFIG']['SWIFT_IP']) + swiftPort = str(config['CONFIG']['SWIFT_PORT']) + self.gps = Location.Location(swiftIP, swiftPort) + self.gps.start_GPS() + + self.speeds = [0,0] + self.errorAccumulation = 0.0 + + #starts the thread that sends wheel speeds + self.running = True + t = Thread(target=self.sendSpeed, name=('send wheel speeds'), args=()) + t.daemon = True + t.start() + + + def startMap(self, func, seconds): + def func_wrapper(): + self.startMap(func, seconds) + func() + t = Timer(seconds, func_wrapper) + t.start() + return t + + def updateMap(self): + self.mapServer.update_rover_coords([self.gps.latitude, self.gps.longitude]) + + + #Every 100ms, send the current left and right wheel speeds to the mbeds + def sendSpeed(self): + while self.running: + ls = int(self.speeds[0]) + rs = int(self.speeds[1]) + UDPOut.sendWheelSpeeds(self.mbedIP, self.mbedPort, ls,ls,ls, rs,rs,rs) + sleep(.1) + + #time in milliseconds + #error in degrees + #Gets adjusted speeds based off of error and how long its been off (uses p and i) + def getSpeeds(self,speed, error, time, kp = .35, ki=.000035): + values = [0,0] + + #p and i constants if doing a pivot turn + if speed == 0: + kp = .9 + ki = .001 + + #Updates the error accumulation + self.errorAccumulation += error * time + + #Gets the adjusted speed values + values[0] = speed + (error * kp + self.errorAccumulation * ki) + values[1] = speed - (error * kp + self.errorAccumulation * ki) + + #Gets the maximum speed values depending if it is pivoting or not + min = speed - 30 + max = speed + 30 + if speed == 0: + max += 40 + min -= 40 + if max > 90: + max=90 + if min < -90: + min = -90 + + #Makes sure the adjusted speed values are within the max and mins + if values[0] > max: + values[0] = max + elif values[0] < min: + values[0] = min + if values[1] > max: + values[1] = max + elif values[1] < min: + values[1] = min + + #Makes sure the speeds are >10 or <-10. Wheels lock up if the speeds are <10 and >-10 + if values[0] <= 0 and values[0] > -10: + values[0] = -10 + elif values[0] > 0 and values[0] < 10: + values[0] = 10 + + if values[1] <= 0 and values[1] > -10: + values[1] = -10 + elif values[1] > 0 and values[1] < 10: + values[1] = 10 + + if values[0] <= 0 and values[0] > -40 and speed == 0: + values[0] = -40 + elif values[0] > 0 and values[0] < 40 and speed == 0: + values[0] = 40 + + if values[1] <= 0 and values[1] > -40 and speed == 0: + values[1] = -40 + elif values[1] > 0 and values[1] < 40 and speed == 0: + values[1] = 40 + + return values + + #Cleaner way to print out the wheel speeds + def printSpeeds(self): + print("Left wheels: ", round(self.speeds[0],1)) + print("Right wheels: ", round(self.speeds[1],1)) + + #Drives along a given list of GPS coordinates while looking for the given ar markers + #Keep id2 at -1 if looking for one post, set id1 to -1 if you aren't looking for AR markers + def driveAlongCoordinates(self, locations, id1, id2=-1): + #Starts the GPS + self.gps.start_GPS_thread() + print('Waiting for GPS connection...') + #while self.gps.all_zero: + # continue + print('Connected to GPS') + + #backs up and turns to avoid running into the last detected sign. Also allows it to get a lock on heading + if(id1 > -1): + self.speeds = [-60,-60] + self.printSpeeds() + sleep(2) + self.speeds = [0,0] + self.printSpeeds() + sleep(2) + self.speeds = [80,20] + self.printSpeeds() + sleep(4) + else: + self.speeds = (self.baseSpeed, self.baseSpeed) + self.printSpeeds() + sleep(3) + + #navigates to each location + for l in locations: + self.errorAccumulation = 0 + while self.gps.distance_to(l[0], l[1]) > .0025: #.0025km + bearingTo = self.gps.bearing_to(l[0], l[1]) + print(self.gps.distance_to(l[0], l[1]) ) + self.speeds = self.getSpeeds(self.baseSpeed, bearingTo, 100) #It will sleep for 100ms + sleep(.1) #Sleeps for 100ms + self.printSpeeds() + + if(id1 != -1 and self.tracker.findMarker(id1, id2)): + self.gps.stop_GPS_thread() + print('Found Marker!') + self.speeds = [0,0] + return True + + print('Made it to location without seeing marker(s). Starting search algorithim. . .') + self.speeds = [0,0] + + #Once location is reached and the tag is not found, get the list of coordinates from the search + #algorithim and loop from line 167 until tag is found. + + #Clear location and populate it with the coordinates from the search algorithm + + locations.clear() + + for l in locations: + self.errorAccumulation = 0 + while self.gps.distance_to(l[0], l[1]) > .0025: #.0025km + bearingTo = self.gps.bearing_to(l[0], l[1]) + print(self.gps.distance_to(l[0], l[1]) ) + self.speeds = self.getSpeeds(self.baseSpeed, bearingTo, 100) #It will sleep for 100ms + sleep(.1) #Sleeps for 100ms + self.printSpeeds() + + if(id1 != -1 and self.tracker.findMarker(id1, id2)): + self.gps.stop_GPS_thread() + print('Found Marker!') + self.speeds = [0,0] + return True + del l[0] + del l[0] + + self.gps.stop_GPS_thread() + print('Completed search without seeing marker(s)') + self.speeds = [0,0] + + return False + + def trackARMarker(self, id1, id2=-1): + stopDistance = 350 #stops when 250cm from markers TODO make sure rover doesn't stop too far away with huddlys + timesNotFound = -1 + self.tracker.findMarker(id1, id2, cameras=1) #Gets and initial angle from the main camera + self.errorAccumulation = 0 + + count = 0 + #Centers the middle camera with the tag + while self.tracker.angleToMarker > 14 or self.tracker.angleToMarker < -14: + if self.tracker.findMarker(id1, id2, cameras=1): #Only looking with the center camera right now + if timesNotFound == -1: + self.speeds = [0,0] + sleep(.5) + self.speeds = [self.baseSpeed, self.baseSpeed] + sleep(.8) + self.speeds = [0,0] + else: + self.speeds = self.getSpeeds(0, self.tracker.angleToMarker, 100) + print(self.tracker.angleToMarker, " ", self.tracker.distanceToMarker) + timesNotFound = 0 + elif timesNotFound == -1: #Never seen the tag with the main camera + if(math.ceil(int(count/20)/5) % 2 == 1): + self.speeds = [self.baseSpeed+5,-self.baseSpeed-5] + else: + self.speeds = [-self.baseSpeed-5,self.baseSpeed+5] + elif timesNotFound < 15: #Lost the tag for less than 1.5 seconds after seeing it with the main camera + timesNotFound += 1 + print(f"lost tag {timesNotFound} times") + else: + self.speeds = [0,0] + print("lost it") #TODO this is bad + timesNotFound = -1 + #return False + self.printSpeeds() + sleep(.1) + count+=1 + self.speeds = [0,0] + sleep(.5) + + if id2 == -1: + self.errorAccumulation = 0 + print("Locked on and ready to track") + + #Tracks down the tag + while self.tracker.distanceToMarker > stopDistance or self.tracker.distanceToMarker == -1: #-1 means we lost the tag + markerFound = self.tracker.findMarker(id1, cameras = 1) #Looks for the tag + + if self.tracker.distanceToMarker > stopDistance: + self.speeds = self.getSpeeds(self.baseSpeed-8, self.tracker.angleToMarker, 100, kp = .5, ki = .0001) + timesNotFound = 0 + print(f"Tag is {self.tracker.distanceToMarker}cm away at {self.tracker.angleToMarker} degrees") + + elif self.tracker.distanceToMarker == -1 and timesNotFound < 10: + timesNotFound += 1 + print(f"lost tag {timesNotFound} times") + + elif self.tracker.distanceToMarker == -1: + self.speeds = [0,0] + print("Lost tag") + return False #TODO this is bad + + self.printSpeeds() + sleep(.1) + + #We scored! + self.speeds = [0,0] + print("In range of the tag!") + return True + else: + #Gets the coords to the point that is 4m infront of the gate posts (get_coordinates expects distance in km) + coords = self.gps.get_coordinates(self.tracker.distanceToMarker/100000.0+.004, self.tracker.angleToMarker) + + self.speeds = [self.baseSpeed, self.baseSpeed] + sleep(5) + + #TODO: test this more after getting the new GPS + ''' + #Rover hasn't been moving for a bit so moves to get correct bearing + self.speeds = [-self.baseSpeed, -self.baseSpeed] + self.printSpeeds() + sleep(2) + self.speeds = [0,0] + self.printSpeeds() + sleep(1) + self.speeds = [self.baseSpeed, self.baseSpeed] + self.printSpeeds() + sleep(2) + + #Drives to the calculated location + while self.gps.distance_to(coords[0], coords[1]) > .003: #TODO: might want to adjust distance here... + bearingTo = self.gps.bearing_to(coords[0], coords[1]) + print(self.gps.distance_to(coords[0], coords[1]) ) + self.speeds = self.getSpeeds(self.baseSpeed, bearingTo, 100) #It will sleep for 100ms + sleep(.1) #Sleeps for 100ms + self.printSpeeds() + ''' + + + diff --git a/libs/Drive.py b/libs/Drive.py index 13334ca..f06a8cf 100644 --- a/libs/Drive.py +++ b/libs/Drive.py @@ -4,8 +4,10 @@ import os os.chdir(os.path.dirname(os.path.abspath(__file__))) import math +import numpy as np from time import sleep from nis import maps +from math import cos, sin, pi import sys sys.path.append('../../Mission Control/RoverMap/') @@ -137,9 +139,11 @@ def printSpeeds(self): print("Left wheels: ", round(self.speeds[0],1)) print("Right wheels: ", round(self.speeds[1],1)) + + #Drives along a given list of GPS coordinates while looking for the given ar markers #Keep id2 at -1 if looking for one post, set id1 to -1 if you aren't looking for AR markers - def driveAlongCoordinates(self, locations, id1, id2=-1): + def driveAlongCoordinates(self, locations, radius, id1, id2=-1): #Starts the GPS self.gps.start_GPS_thread() print('Waiting for GPS connection...') @@ -179,11 +183,65 @@ def driveAlongCoordinates(self, locations, id1, id2=-1): self.speeds = [0,0] return True - self.gps.stop_GPS_thread() - print('Made it to location without seeing marker(s)') + print('Made it to location without seeing marker(s). Starting search algorithim. . .') self.speeds = [0,0] + #Aruco tag is not found so search algorithim is started + + #Run if locations contains a radius value as to not run the search algorithim if it + #is not needed + if (radius != -1): + + #Run search algorithim + x = locations[0] + y = locations[1] + #locations[2] will hold the given radius + radius = radius/(np.pi*10) + #clear locations to remove the given gps coordinate to populate with coordinates generated + #by the search algorithim + locations.clear() + + #repeatedly adds x and y values of generated coordinates to locations. Starts with the first generated + #point. It skips the starting point as the rover has already traveled to that position + for theta in np.linspace(10*np.pi,0, 50): + + #TOTAL RADIUS OF CIRCLE + # DETERMINED BY DESIRED RADIUS/(PI*10) + r = ((theta)*radius) + + x_pos = r*cos(theta)+x + y_pos = r*sin(theta)+y + + locations.append(x_pos) + locations.append(y_pos) + + #loop while locations still has values in it + while len(locations) > 0: + self.errorAccumulation = 0 + while self.gps.distance_to(locations[0], locations[1]) > .0025: #.0025km + bearingTo = self.gps.bearing_to(locations[0], locations[1]) + print(self.gps.distance_to(locations[0], locations[1]) ) + self.speeds = self.getSpeeds(self.baseSpeed, bearingTo, 100) #It will sleep for 100ms + sleep(.1) #Sleeps for 100ms + self.printSpeeds() + + if(id1 != -1 and self.tracker.findMarker(id1, id2)): + self.gps.stop_GPS_thread() + print('Found Marker!') + self.speeds = [0,0] + return True + #removes the x and y values of the coordinate just visited if the aruco tag was not found + del l[0] + del l[0] + + print('Completed search without seeing marker(s)') + self.speeds = [0,0] + + self.gps.stop_GPS_thread() + return False + + def trackARMarker(self, id1, id2=-1): stopDistance = 350 #stops when 250cm from markers TODO make sure rover doesn't stop too far away with huddlys timesNotFound = -1 diff --git a/main.py b/main.py index cfde983..48b2b83 100644 --- a/main.py +++ b/main.py @@ -23,6 +23,7 @@ def flash(): argParser.add_argument("cameraInput", type=int, help="takes a number representing which camera to use") argParser.add_argument("-id", "--ids", type=int, help="takes either 1 or 2 id values, defaults to -1 if id not assigned", nargs='+') argParser.add_argument("-ll", "--latLong", type=str, help="takes a filename for a text file, then reads that file for latlong coordinates") +argParser.add_argument("-r", "--radius", type=int, help="takes the radius for the Aruco tag search") args = argParser.parse_args() #Gets a list of coordinates from user and drives to them and then tracks the tag #Set id1 to -1 if not looking for a tag @@ -30,6 +31,7 @@ def drive(rover): global flashing idList = [-1,-1] locations = [] + radius = -1 if args.ids is not None: for i in range(len(args.ids)): @@ -55,9 +57,12 @@ def drive(rover): locations.append(coords) f.close() + if args.radius is not None: + radius = args.radius + flashing = False UDPOut.sendLED(mbedIP, mbedPort, 'r') - found = rover.driveAlongCoordinates(locations,id1, id2) + found = rover.driveAlongCoordinates(locations, radius, id1, id2) if id1 != -1: rover.trackARMarker(id1, id2)