class Robot: def __init__(self): self.isLost = False self.nextSearchDirection = 0 # (left: 0, right: 1) self.veerSpeed = 40 self.maxSpeed = 100 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1] # check if currently reading green tape def checkGreen(self): r, g, b, c = self.tcs.get_raw_data() return (g > r and g > b) def checkRed(self): r, g, b, c = self.tcs.get_raw_data() return (r > g and r > b) # set motor speeds def motors(self, lspeed, rspeed): self.aStar.motors(lspeed, rspeed) def goForward(self): self.motors(self.maxSpeed, self.maxSpeed) def stop(self): self.motors(0, 0) def veerLeft(self): self.motors(self.veerSpeed, self.maxSpeed) def veerRight(self): self.motors(self.maxSpeed, self.veerSpeed) def getTime(self): return time.time() def playNotes(self): self.aStar.play_notes("o4l16ceg>c8") # turn leds on or off def leds(self, red, yellow, green): self.aStar.leds(red, yellow, green) def readAnalog(self): return self.aStar.read_analog() def readBatteryMillivolts(self): return self.aStar.read_battery_millivolts() def readButtons(self): return self.aStar.read_buttons() def readEncoders(self): encoders = self.aStar.read_encoders() return (encoders[0] - self.initialLeftCount, encoders[1] - self.initialRightCount) def printColorInfo(self): r, g, b, c = self.tcs.get_raw_data() temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b) lux = Adafruit_TCS34725.calculate_lux(r, g, b) print('Color: r={0} g={1} b={2} temp={3} lux={4}'.format( r, g, b, temp, lux)) def turnOffInterruptsRGB(self): self.tcs.set_interrupt(False) def turnOnInterruptsRGB(self): self.tcs.set_interrupt(True) # rgb sensor is enabled by default in the constructor method def enableRGB(self): self.tcs.enable() def disableRGB(self): self.tcs.disable() def turnLeft(self, count): while (self.readEncoders()[0] % 744 > (-1) * count and self.readEncoders()[1] % 744 < count): if (self.checkGreen()): return 1 break self.motors(0, self.maxSpeed) self.stop() return 0 def turnRight(self, count): while (self.readEncoders()[0] % 744 < count and self.readEncoders()[1] % 744 > (-1) * count): if (self.checkGreen()): return 1 break self.motors(self.maxSpeed, 0) self.stop() return 0 def changeCount(self, count): if (count != 0): count = count / 2 def calibrate(self): count = 744 while (self.checkGreen() == False): if (self.turnLeft(count)): pass elif (self.turnRight(count)): pass self.changeCount(count) # end while self.goForwardtwo() def turn90Left(self): initEncoders = self.readEncoders() while (self.readEncoders()[0] > initEncoders[0] - 744 and self.readEncoders()[1] < initEncoders[1] + 744): self.motors(self.maxSpeed * (-1), self.maxSpeed) self.stop() def turn90Right(self): initEncoders = self.readEncoders() while (self.readEncoders()[1] > initEncoders[1] - 744 and self.readEncoders()[0] < initEncoders[0] + 744): self.motors(self.maxSpeed, self.maxSpeed * (-1)) self.stop() def goForwardtwo(self): self.motors(self.maxSpeed, self.maxSpeed) x = self.maxSpeed diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0]) while (diff > 2): diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0]) x -= 1 if (self.readEncoders()[1] > self.readEncoders()[0]): self.motors(self.maxSpeed, x) time.sleep(1.0 / 1000.0) elif (self.readEncoders()[0] > self.readEncoders()[1]): self.motors(x, self.maxSpeed) time.sleep(1.0 / 1000.0) self.motors(self.maxSpeed, self.maxSpeed) ''' def calibrate(self): while(self.checkGreen() == False): for x in range(100, -20, -1): self.motors(100 if x+40>100 else x+40,x) ''' def adjustIntersection(self): saveEncoders = self.readEncoders() while (self.readEncoders()[0] - saveEncoders[0] < 648 or self.readEncoders()[1] - saveEncoders[1] < 648): self.goForwardtwo() # recalibrate the robot onto the path def calibrateDirection(self): calibrated = False turnAmount = 200 # initial encoder count inRed = False while (self.checkGreen() == False): # turn the robot one direction if (self.nextSearchDirection == 0): self.veerLeft() else: self.veerRight() # switch turn direction for next iteration self.nextSearchDirection = self.nextSearchDirection ^ 1 # wait to see if direction was correct while (self.readEncoders()[self.nextSearchDirection] < turnAmount): if (self.checkGreen()): self.adjustIntersection() while (not self.checkGreen()): if (self.nextSearchDirection == 0): self.motors(-100, 100) else: self.motors(100, -100) slapEncoders = self.readEncoders() if (self.nextSearchDirection == 0): while (self.readEncoders()[0] - slapEncoders[0] < 81): self.motors(-100, 100) self.stop() else: while (self.readEncoders()[1] - slapEncoders[1] < 81): self.motors(100, -100) self.stop() break
class Robot: def __init__(self): self.isLost = False self.nextSearchDirection = 0 # (left: 0, right: 1) self.veerSpeed = 30 self.maxSpeed = 70 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1] # check if currently reading green tape def checkGreen(self): r, g, b, c = self.tcs.get_raw_data() return (g > r and g > b) def checkRed(self): r, g, b, c = self.tcs.get_raw_data() return (r > 1.6 * g or r > 1.6 * b) and r > 50 # set motor speeds def motors(self, lspeed, rspeed): self.aStar.motors(lspeed, rspeed) def goForward(self): self.motors(self.maxSpeed, self.maxSpeed) def stop(self): self.motors(0, 0) def veerLeft(self): self.motors(self.veerSpeed, self.maxSpeed) def veerRight(self): self.motors(self.maxSpeed, self.veerSpeed) def getTime(self): return time.time() def playNotes(self): self.aStar.play_notes("o4l16ceg>c8") # turn leds on or off def leds(self, red, yellow, green): self.aStar.leds(red, yellow, green) def readAnalog(self): return self.aStar.read_analog() def readBatteryMillivolts(self): return self.aStar.read_battery_millivolts() def readButtons(self): return self.aStar.read_buttons() def readEncoders(self): encoders = self.aStar.read_encoders() return (encoders[0] - self.initialLeftCount, encoders[1] - self.initialRightCount) def printColorInfo(self): r, g, b, c = self.tcs.get_raw_data() temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b) lux = Adafruit_TCS34725.calculate_lux(r, g, b) print('Color: r={0} g={1} b={2} temp={3} lux={4}'.format( r, g, b, temp, lux)) def turnOffInterruptsRGB(self): self.tcs.set_interrupt(False) def turnOnInterruptsRGB(self): self.tcs.set_interrupt(True) # rgb sensor is enabled by default in the constructor method def enableRGB(self): self.tcs.enable() def disableRGB(self): self.tcs.disable() def turnLeft(self, count): while (self.readEncoders()[0] % 744 > (-1) * count and self.readEncoders()[1] % 744 < count): if (self.checkGreen()): return 1 break self.motors(0, self.maxSpeed) self.stop() return 0 def turnRight(self, count): while (self.readEncoders()[0] % 744 < count and self.readEncoders()[1] % 744 > (-1) * count): if (self.checkGreen()): return 1 break self.motors(self.maxSpeed, 0) self.stop() return 0 def changeCount(self, count): if (count != 0): count = count / 2 def calibrate(self): count = 744 while (self.checkGreen() == False): if (self.turnLeft(count)): pass elif (self.turnRight(count)): pass self.changeCount(count) # end while self.goForwardtwo() def greenIsLeft(self): initEncoders = self.readEncoders() while (self.readEncoders()[0] > initEncoders[0] - 100 and self.readEncoders()[1] < initEncoders[1] + 100 and self.checkGreen() == False): self.motors(-50, 50) self.stop() if (self.checkGreen()): return True else: return False def greenIsRight(self): initEncoders = self.readEncoders() while (self.readEncoders()[1] > initEncoders[1] - 100 and self.readEncoders()[0] < initEncoders[0] + 100 and self.checkGreen() == False): self.motors(50, -50) self.stop() if (self.checkGreen()): return True else: return False def adjustSlightly(self, direction): counts = 50 initEncoders = self.readEncoders() if (direction == 'left'): while (self.readEncoders()[1] < initEncoders[1] + counts): self.motors(-50, 50) else: while (self.readEncoders()[0] < initEncoders[0] + counts): self.motors(50, -50) def calibrateTwo(self): self.stop() if (self.greenIsLeft()): self.adjustSlightly('left') elif (self.greenIsRight()): self.adjustSlightly('right') elif (self.checkRed()): pass else: print("Unable to find green") ''' def calibrateTwo(self): self.sweepsForGreen(80) ''' def turnCounts(self, direction, counts): initEncoders = self.readEncoders() if (direction == 'left'): while (self.readEncoders()[0] > initEncoders[0] - counts and self.readEncoders()[1] < initEncoders[1] + counts): self.motors(self.maxSpeed * (-1), self.maxSpeed) elif (direction == 'right'): while (self.readEncoders()[1] > initEncoders[1] - counts and self.readEncoders()[0] < initEncoders[0] + counts): self.motors(self.maxSpeed, self.maxSpeed * (-1)) else: print("Invalid turn direction") self.stop() def turn90Left(self): self.turnCounts('left', 744) def turn90Right(self): self.turnCounts('right', 744) # return true is green is found within a sweep distance, false otherwise def sweepsForGreen(self, counts): # counts is equal to the number of encoders counts at both sides of # facing direction e.g. count = 50 searches 50 counts left and 50 right interval = 10 iterations = math.ceil(counts / interval) initEncoders = self.readEncoders() # sweep left for i in range(1, iterations): self.turnCounts('left', interval) if (self.checkGreen()): return True #print(self.readEncoders()[1] - initEncoders[1]) #self.turnCounts('right', self.readEncoders()[1] - initEncoders[1]) # sweep right for i in range(1, iterations): self.turnCounts('right', interval) if (self.checkGreen()): return True #print(self.readEncoders()[0] - initEncoders[0]) #self.turnCounts('left', self.readEncoders()[0] - initEncoders[0]) return False def isPathInDirection(self, direction): if (direction == 'forward'): pass elif (direction == 'left'): self.turn90Left() elif (direction == 'right'): self.turn90Right() return self.sweepsForGreen(120) def getValidPaths(self): forward = self.isPathInDirection('forward') self.stop() time.sleep(0.5) left = self.isPathInDirection('left') self.stop() time.sleep(0.5) self.turn90Right() right = self.isPathInDirection('right') self.stop() time.sleep(0.5) self.turn90Left() time.sleep(0.5) return (left, forward, right) def noValidPaths(pathTuple): return not (pathTuple[0] or pathTuple[1] or pathTuple[2]) def goForwardtwo(self): self.motors(self.maxSpeed, self.maxSpeed) x = self.maxSpeed diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0]) while (diff > 2): diff = math.fabs(self.readEncoders()[1] - self.readEncoders()[0]) x -= 1 if (self.readEncoders()[1] > self.readEncoders()[0]): self.motors(self.maxSpeed, x) time.sleep(1.0 / 1000.0) elif (self.readEncoders()[0] > self.readEncoders()[1]): self.motors(x, self.maxSpeed) time.sleep(1.0 / 1000.0) self.motors(self.maxSpeed, self.maxSpeed) ''' def calibrate(self): while(self.checkGreen() == False): for x in range(100, -20, -1): self.motors(100 if x+40>100 else x+40,x) ''' def adjustIntersection(self): saveEncoders = self.readEncoders() while (self.readEncoders()[0] - saveEncoders[0] < 700 and self.readEncoders()[1] - saveEncoders[1] < 700): self.goForwardtwo() print("Centered at intersection") # recalibrate the robot onto the path def calibrateDirection(self): calibrated = False turnAmount = 500 # initial encoder count print("in calibrate") while (calibrated == False): # turn the robot one direction if (self.nextSearchDirection == 0): self.veerLeft() else: self.veerRight() # switch turn direction for next iteration self.nextSearchDirection = self.nextSearchDirection ^ 1 print("in first while") recordEncoder = self.readEncoders()[self.nextSearchDirection] # wait to see if direction was correct while (self.readEncoders()[self.nextSearchDirection] - recordEncoder < turnAmount): print("in second while") if (self.checkGreen()): # creep forward until centered self.adjustIntersection() print("done centering") # reposition front of robot onto green edge while (self.checkGreen() == False): if (self.nextSearchDirection == 0): print("repositioning [left] to green edge") self.veerLeft() else: print("repositioning [right] to green edge") self.veerRight() print("done repositioning to green edge") calibrated = True ''' # rotate half an inch until sensor is centered on tape slapEncoders = self.readEncoders() if(self.nextSearchDirection == 0): while(self.readEncoders()[1] - slapEncoders[1] < 81): print("rotating left half an inch") self.motors(-100, 100) self.stop() else: while(self.readEncoders()[0] - slapEncoders[0] < 81): print("rotating right half an inch") self.motors(100, -100) self.stop() ''' break else: pass # not green, continue to veer
class Robot: # initialize robot def __init__(self): self.directionFacing = 'north' self.maxSpeed = 100 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1] # makes multiple readings with RGB sensor and returns the average values def getColorAverage(self): numIterations = 3 averageG = averageR = averageB = 0 for i in range(0,numIterations): r, g, b, c = self.tcs.get_raw_data() averageG += g averageR += r averageB += b averageG /= numIterations averageR /= numIterations averageB /= numIterations return (averageR, averageG, averageB) # check whether RGB sensor is reading green def checkGreen(self): averageColors = self.getColorAverage() return (averageColors[1] > averageColors[0]) and (averageColors[1] > averageColors[2]) and averageColors[1] > 20 # check whether RGB sensor is reading red def checkRed(self): averageColors = self.getColorAverage() return (averageColors[0] > averageColors[1]) and (averageColors[0] > averageColors[2]) and averageColors[0] > 20 # check whether RGB sensor is reading purple def checkPurple(self): averageColors = self.getColorAverage() return (math.fabs(averageColors[0] - averageColors[2]) < 3) and (averageColors[1] / averageColors[0] < 0.6) # grab tuple containing RGB values (r, g, b, clear) def getColors(self): return self.tcs.get_raw_data() # set motor speeds of robot wheels def motors(self, lspeed, rspeed): self.aStar.motors(lspeed, rspeed) # set both motor speeds to max speed forward direction def goForward(self): self.motors(self.maxSpeed, self.maxSpeed) # set both motor speeds to zero def stop(self): self.motors(0, 0) # function used to getting the current time def getTime(self): return time.time() # play music notes on arduino buzzer def playNotes(self): self.aStar.play_notes("o4l16ceg>c8") # turn leds on or off def leds(self, red, yellow, green): self.aStar.leds(red, yellow, green) # reads analog information of arduino def readAnalog(self): return self.aStar.read_analog() # get the current reading of the robot battery def readBatteryMillivolts(self): return self.aStar.read_battery_millivolts() # get the boolean values of buttons on the arduino (A, B, C) def readButtons(self): return self.aStar.read_buttons() # reads the encoder information on the arduino wheels and returns a tuple (INT, INT) # uses relative encoder information based on class initialization def readEncoders(self): encoders = self.aStar.read_encoders() return (encoders[0] - self.initialLeftCount, encoders[1] - self.initialRightCount) # print r, g, b, temperature, and lux values of rgb sensor def printColorInfo(self): r, g, b, c = self.tcs.get_raw_data() temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b) lux = Adafruit_TCS34725.calculate_lux(r, g, b) print('Color: r={0} g={1} b={2} temp={3} lux={4}'.format(r, g, b, temp, lux)) def turnOffInterruptsRGB(self): self.tcs.set_interrupt(False) def turnOnInterruptsRGB(self): self.tcs.set_interrupt(True) # rgb sensor is enabled by default in the constructor method def enableRGB(self): self.tcs.enable() def disableRGB(self): self.tcs.disable() # used to calibrate direction to green facing def calibrateTwo(self): self.sweepsForGreen(100) # rotate bot a specified number of encoder counts def turnCounts(self, direction, counts): initEncoders = self.readEncoders() if(direction == 'left'): while( self.readEncoders()[0] > initEncoders[0] - counts and self.readEncoders()[1] < initEncoders[1] + counts): self.motors(self.maxSpeed * (-1), self.maxSpeed) elif(direction == 'right'): while( self.readEncoders()[1] > initEncoders[1] - counts and self.readEncoders()[0] < initEncoders[0] + counts): self.motors(self.maxSpeed, self.maxSpeed * (-1)) else: print("Invalid turn direction") self.stop() # rotate robot 90 degrees left def turn90Left(self): self.turnCounts('left', 744) def turn90Right(self): self.turnCounts('right', 744) # move robot forward while calibrating the difference between the # wheel encoders to be within an epsilon of equivalence def goForwardtwo(self, offset): x = self.maxSpeed self.motors(int(100),int(100)) grabEncoders = self.updateEncoders(offset) diff = math.fabs(grabEncoders[0] - grabEncoders[1]) # print(diff) while(diff > 3): grabEncoders = self.updateEncoders(offset) diff = math.fabs(grabEncoders[0] - grabEncoders[1]) x-=1.0 if(grabEncoders[0] < grabEncoders[1]): self.motors(int(self.maxSpeed), int(x)) time.sleep(2.0/1000.0) elif(grabEncoders[0] > grabEncoders[1]): self.motors(int(x), int(self.maxSpeed)) time.sleep(2.0/1000.0) self.motors(int(self.maxSpeed), int(self.maxSpeed)) # get encoder offset (if one wheel is negative and the other is positive after a rotation) def zeroEncoders(self): return([self.aStar.read_encoders()[0]%32768, self.aStar.read_encoders()[1]%32768]) # used for iterative tracking of encoder information from some original state # e.g. baseline = self.updateEncoders()...do loop...currentState = self.updateEncoders... compare against baseline def updateEncoders(self, offset): return([(self.aStar.read_encoders()[0]-offset[0])%32768, (self.aStar.read_encoders()[1]-offset[1])%32768]) # look for green a number of rotation encoder counts in a particular direction def sweep(self, direction, counts): interval = 10 iterations = math.ceil(counts / interval) for i in range (1, iterations): self.turnCounts(direction, interval) if(self.checkGreen()): return True return False # return true is green is found within a sweep distance, false otherwise def sweepsForGreen(self, counts): # counts is equal to the number of encoders counts at both sides of # facing direction e.g. count = 50 searches 50 counts left and 50 right initEncoders = self.readEncoders() self.sweep('left', counts) self.turnCounts('right', self.readEncoders()[1] - initEncoders[1]) self.sweep('right', counts) self.turnCounts('left', self.readEncoders()[0] - initEncoders[0]) return False # check where there are paths at a given intersection, return a list def getValidPaths(self): directions = list() self.calibrateTwo() if(self.checkGreen()): directions.append('forward') self.turn('left') if(self.checkGreen()): directions.append('left') self.turn('backward') if(self.checkGreen()): directions.append('right') self.turn('left') print(directions) return directions # center robot at intersection after intersection is detected def adjustIntersection(self): saveEncoders = self.readEncoders() while(self.readEncoders()[0] - saveEncoders[0] < 700 and self.readEncoders()[1] - saveEncoders[1] < 700): self.goForward() print("Centered at intersection") # takes initial encoder values and newly recorded encoder values # and calculates the average of both to estimate the total forward distance def calculateForwardDistance(self, initialEncoderInfo): newEncoders = self.readEncoders() leftDistance = newEncoders[0] - initialEncoderInfo[0] rightDistance = newEncoders[1] - initialEncoderInfo[1] averageDistance = (leftDistance / 2) + (rightDistance / 2) return averageDistance # calculates current location based on a coordinate system def calculatePosition(self, lastPosition, directionFacing, forwardDistance): x = lastPosition[0] y = lastPosition[1] if(directionFacing == 'north'): return (x, y + forwardDistance) elif(directionFacing == 'west'): return (x - forwardDistance, y) elif(directionFacing == 'east'): return (x + forwardDistance, y) elif(directionFacing == 'south'): return (x, y - forwardDistance) else: print('invalid direction in calculatePosition()') return (0, 0) # we should never end up back at the starting position # calculate the new facing direction after turn def directionFacingAfterTurn(self, directionTurning, directionCurrentlyFacing): directions = ['north', 'west', 'south', 'east'] index = 0 if(directionTurning == 'left'): index = (directions.index(directionCurrentlyFacing) + 1) % 4 elif(directionTurning == 'right'): index = (directions.index(directionCurrentlyFacing) - 1) % 4 elif(directionTurning == 'forward'): index = (directions.index(directionCurrentlyFacing) + 0) % 4 elif(directionTurning == 'backward'): index = (directions.index(directionCurrentlyFacing) + 2) % 4 else: print('Error in directionFacingAfterTurn, invalid directionCurrentlyFacing value') return directions[index] # turn robot in a specified direction def turn(self, direction): if(direction == 'left'): self.turn90Left() self.calibrateTwo() elif(direction == 'right'): self.turn90Right() self.calibrateTwo() elif(direction == 'forward'): pass elif(direction == 'backward'): self.turn90Left() self.turn90Left() self.calibrateTwo() else: print("invalid direction in turn()") self.directionFacing = self.directionFacingAfterTurn(direction, self.directionFacing) # head back to previous intersection def goToPreviousIntersection(self): while(not (self.checkRed())): if(self.checkGreen()): self.goForwardtwo(offset) else: self.calibrateTwo() offset = self.zeroEncoders() self.adjustIntersection() offset = self.zeroEncoders() # traverse through a maze using backtracking def completeMaze(self, previousPositions = list()): mazeSolved = False initialEncoderInfo = self.readEncoders() forwardDistance = 0 # record initial position as a coordinate if list is empty, it should never be removed # and will always be the only position left in the list if all positions are popped offset = self.zeroEncoders() if (not previousPositions): previousPositions.append((0, 0)) self.directionFacing = 'north' # continue until maze exit is found while(not mazeSolved): # follow the green-brick road if(self.checkGreen()): self.goForwardtwo(offset) # found intersection elif(self.checkRed()): self.adjustIntersection() forwardDistance = self.calculateForwardDistance(initialEncoderInfo) # In python, the [-1] references the last index in a list currentPosition = self.calculatePosition(previousPositions[-1], self.directionFacing, forwardDistance) # if found end of maze, return if(self.checkPurple()): return True paths = self.getValidPaths() offset = self.zeroEncoders() # if no paths were detected, must be deadend if(not paths): pass else: # try eacn path (alter state), recurse, go back to starting position (revert state) for path in paths: self.turn(path) # change state offset = self.zeroEncoders() mazeSolved = self.completeMaze(previousPositions) # recursive call if(mazeSolved): return True # in order to get back to the original facing position, repeat original turn direction if(path != 'forward'): self.turn(path) # revert state offset = self.zeroEncoders() # forward is an exception, if returning from forward, stop at original position and turn around else: self.turn('backward') offset = self.zeroEncoders() self.turn('backward') # turn towards last intersection offset = self.zeroEncoders() self.goToPreviousIntersection() return False # end if checkRed() else: self.calibrateTwo() offset = self.zeroEncoders() # end while return False