def dance1(): snakyFinch = Finch() for i in range(5): snakyFinch.wheels(1, 1) sleep(1) snakyFinch.wheels(0, 1) sleep(1) snakyFinch.wheels(1, 0) sleep(1) snakyFinch.wheels(-1, -1) sleep(0.5) snakyFinch.wheels(0.2, -1) sleep(1) snakyFinch.wheels(0.3, 0.3) sleep(2.5) if i == 4: snakyFinch.close();
def dance3(): snakyFinch = Finch() for i in range(5): snakyFinch.wheels(1, 0) sleep(1) snakyFinch.wheels(-1, 1) sleep(1) snakyFinch.wheels(1, 0) sleep(1) snakyFinch.wheels(-1, 0) sleep(0.5) snakyFinch.wheels(-0.9, 1) sleep(1) snakyFinch.wheels(0.75, 0.2) sleep(2.5) if i == 4: snakyFinch.close();
def q2(): from time import sleep from finch import Finch # this code won't actually work unless you install the finch package # or you can replace this with the ebot equivalent if you want # to run the code # Whatever the case, the concept is the same finch = Finch() finch.wheels(0.2,0.2) leftObstacle, rightObstacle = finch.obstacle() while leftObstacle == False and rightObstacle == False: finch.wheels(0.2,0.2) finch.halt() finch.close()
def dance2(): snakyFinch = Finch() for i in range(5): snakyFinch.wheels(1, 1) sleep(1) snakyFinch.wheels(0, -1) sleep(1) snakyFinch.wheels(1, 0) sleep(1) snakyFinch.wheels(1, -1) sleep(0.5) snakyFinch.wheels(0.5, 1) sleep(1) snakyFinch.wheels(0.75, -0.3) sleep(2.5) if i == 4: snakyFinch.close();
# *just* flipped us upside down, so take action! # Choose an answer randomly. answer_index = randrange(0, len(answers)) # Print the answer. print(answers[answer_index]) # Remember that we're already upside down for the next time # we check. was_upside_down_before = True else: # Based on x, y and z, we're *not* upside down. # Red LED (hints to the user that we're right-side up and not # ready to print an answer). tweety.led(255, 0, 0) # Remember that we turned right-side up again. was_upside_down_before = False # If we sense obstacles, then it's time to close. left_obstacle, right_obstacle = tweety.obstacle() # Let the user know the program has ended. print('Thanks for playing. Goodbye!') # Always the last step: Close the connection to our Finch to keep # it happy. tweety.close()
start = time() finch.wheels(0.5, 0.5) sleep(0.1) while True: left_obstacle, right_obstacle = finch.obstacle() if left_obstacle or right_obstacle: half_lap_time = time() - start finch.wheels(0, 0) break print('Obstacle found, backing up') # Move backwards for the same amount of time spent moving forward finch.wheels(-0.5, -0.5) sleep(half_lap_time) laps -= 1 # Now lapswim! while laps > 0: finch.wheels(0.5, 0.5) sleep(half_lap_time) finch.wheels(0, 0) sleep(0.1) finch.wheels(-0.5, -0.5) sleep(half_lap_time) laps -= 1 finch.close()
class myFinch: # Class Initializer # Synopsis - # self.left_wheel, is the speed of the left wheel, set to 0 # self.right_wheel, is the speed of the right wheel, set to 0 # self.tweety, is the Finch robot object which will be manipulated through this class # # Description - # Initialize the class members to a specific value def __init__(self): self.tweety = Finch() # Set obstacle sensors self.left_obst, self.right_obst = self.tweety.obstacle() # Setting initial lighting self.myLights = lighting(self.tweety) # Setting initial acceleration settings self.x, self.y, self.z, self.tap, self.shake = self.tweety.acceleration() # Setting initial wheel speed self.left_wheel = 0.0 self.right_wheel = 0.0 self.tweety.wheels(self.left_wheel, self.right_wheel) def __del__(self): self.tweety.close() # [FUNCTION]Name - setWheels ( left, right ) # Synopsis - # def setWheels ( left, right ) : # left, an double value for the left wheel speed # right, an double value for the right wheel speed # # Description - # Accelerates each wheel at different speeds to obtain the new speed. This can be # adjusted by small increments and also gives each wheel independent ending # speeds. # # Return - # (none) def setWheels ( self, left, right ): self.left_wheel = left self.right_wheel = right self.tweety.wheels(self.left_wheel, self.right_wheel) self.printSpeed() return # setWheels ( left, right ) # def isLight( self ): current_left, current_right = self.tweety.light() print ("Left reading : ",current_left) print ("Right reading : ", current_right) sleep(1) return current_left, current_right return False return True # [FUNCTION]Name - def printSpeed ( ) # Synopsis - # def printSpeed ( ): # (no parameters) # Description - # Upon changing speed with setWheels this will print the individual wheel speed # or both to the console. # # Return - # (none) def printSpeed ( self ): if (self.left_wheel == self.right_wheel): print ("Current speed : ", self.left_wheel) return print("Left speed : ", self.left_wheel) print("Right speed : ", self.right_wheel) return # printSpeed ( self ) # # [FUNCTION]Name - def detectSingleObstacle ( ) # Synopsis - # def detectSingleObstacle ( ): # (no parameters) # Description - # Function will utilize the finch's right and left sensors to see if there is # an obstacale on the left or right. # # Return - # "Left" upon a left_obst sensor being triggered # "right" upon a right_obst sensor being triggered # False otherwise def detectSingleObstacle(self): #while there is no obstacle continue self.left_obst, self.right_obst = self.tweety.obstacle() if self.left_obst: print ( "Left obstacle" ) return "left" elif self.right_obst: print ( "Right obstacle" ) return "right" return False # detectSingleObstacle(self) # # [FUNCTION]Name - def detectWall ( ) # Synopsis - # def detectWall ( ): # (no parameters) # Description - # Function will utilize the finch's right and left sensors to see if there is # an obstacale directly in front of both sensors. # # Return - # True if obstacle detected on both sides. # False if no obstacle detected / only one side detected def detectWall (self): self.left_obst, self.right_obst = self.tweety.obstacle() if (self.left_obst and self.right_obst): print (self.left_obst, self.right_obst) return True return False # detectWall ( self ) # def detectLight(self): while(True): self.myLights.isDifferent() sleep(2) #################################################### ############## GENERAL MOVEMENT #################### #################################################### def reverseLeft( self, t ): self.tweety.led("#800080") self.setWheels(-1.0, -0.5) sleep ( t ) return def reverseRight( self, t ): self.tweety.led("#800080") self.setWheels(-0.5, -1.0) sleep ( t ) return def forLeft( self, t ): self.tweety.led("#800080") self.setWheels( 0.25, 0.5) sleep ( t ) return def forRight( self, t ): self.tweety.led("#800080") self.setWheels( 1.0, 0.5 ) sleep ( t ) return def straight( self, speed, t ): self.tweety.led("#00FF00") self.setWheels( speed, speed ) sleep ( t ) return def reverse( self, speed, t ): self.tweety.led("#800080") self.setWheels( -speed, -speed ) sleep ( t ) return def stop( self ): self.tweety.led("#800080") self.setWheels(0.0, 0.0) return def forceStop(self): self.tweety.led('#FF0000') self.stop() os._exit(0)
if (side == RIGHT): wheels(0.0, R_INITIAL) else: wheels(L_INITIAL, 0.0) #BREAK OUT OF LOOP IF INFINITE SPINNING? while not left_obst or not right_obst: left_obst, right_obst = finchy.obstacle() if right_obst: return RIGHT else: return LEFT ########################################### # # MAIN # ########################################### #initialize the finch & data finchy = Finch() #MENU #function calls here multithread here startFinch(finchy) finchy.close()
class MyRobot: # Constructor, the polarity is here cause some robots may have the # polarity wrong (i.e. right wheel is going in wrong direction) # The 'adjust' variables are to adjust for different speeds in the wheels # it tries to make them in sync. def __init__(self, left, right, inAdjustmentMode=False): self.leftWheel = left self.rightWheel = right self.myBot = Finch() # Keep track of the direction you should try on obstacles self.obstacleDirectionToTry = finchConstants.LEFT self.lastScrapedSide = " " # Keep track of when the last time you moved forward, we use that # to identify sensors self.lastTimeIMovedForward = -1 # Use vars below for logging when self.lastPersistedLeftObstacleState = False self.lastPersistedRightObstacleState = False # Obstacle reading goes haywire... we'll count up readings within # an interval and use the overall score to report self.lastObstacleReading = 0.0 self.lastObstacleReadingLeftScores = [0, 0, 0, 0, 0] self.lastObstacleReadingRightScores = [0, 0, 0, 0, 0] self.scorePosition = 0 self.obstacleNumberOfScores = len(self.lastObstacleReadingLeftScores) self.obstacleState = { "left": False, "leftStateTime": 0.0, "leftElapsedTime": 0.0, "right": False, "rightStateTime": 0.0, "rightElapsedTime": 0.0 } # If true then wheel speed will be adjusted by the constants self.inWheelAdjustmentMode = inAdjustmentMode self.lasttime = time.time() self.update(self.inWheelAdjustmentMode) # Reset the state of the sensors def resetState(self): self.obstacleState["left"] = False self.obstacleState["leftStateTime"] = 0.0 self.obstacleState["leftElapsedTime"] = 0.0 self.obstacleState["right"] = False self.obstacleState["rightStateTime"] = 0.0 self.obstacleState["rightElapsedTime"] = 0.0 self.lastPersistedLeftObstacleState = False self.lastPersistedRightObstacleState = False self.lastObstacleReading = 0.0 self.lastObstacleReadingLeftScores = [0, 0, 0, 0, 0] self.lastObstacleReadingRightScores = [0, 0, 0, 0, 0] self.scorePosition = 0 # This is common routine to update the dictionary 'state' items associated # with the sensors... we need this because we need the state to persist for # a given amount of time before we think of it as 'real' (the bot sensors are flakey) # NOTE: The state will only be updated when wheels are moving, or you pass in the # forceUpdate indicator def updateMyState(self, forceUpdate=False): if self.leftWheel != 0.0 or self.rightWheel != 0.0 or forceUpdate: stateTime = time.time() leftObst, rightObst = self.myBot.obstacle() # Because obstacle readings are erradic I take last 5 readings, and report that # value if sum of array is > 2 then report True else report false if leftObst == True: leftIntValue = 1 else: leftIntValue = 0 if rightObst == True: rightIntValue = 1 else: rightIntValue = 0 indexPosition = self.scorePosition % 5 self.lastObstacleReadingLeftScores[indexPosition] = leftIntValue self.lastObstacleReadingRightScores[indexPosition] = rightIntValue self.scorePosition = indexPosition + 1 lastReadingElapsed = round(stateTime - self.lastObstacleReading, 3) finchClassLogger.debug( "finchClass-updateMyState, lastReadingElapsed: {0}, leftObst: {1} rightObst{2}" .format(lastReadingElapsed, leftObst, rightObst)) # If reading is less than intervals ignore it if lastReadingElapsed < OBSTACLE_READING_DELAY: return self.lastObstacleReading = stateTime # Calculate the value for each of the obstacle sensors leftIntValue = 0 rightIntValue = 0 indexPosition = 0 while indexPosition < self.obstacleNumberOfScores: leftIntValue += self.lastObstacleReadingLeftScores[ indexPosition] rightIntValue += self.lastObstacleReadingRightScores[ indexPosition] indexPosition += 1 if leftIntValue > 2: leftObstacleReading = True else: leftObstacleReading = False if rightIntValue > 2: rightObstacleReading = True else: rightObstacleReading = False finchClassLogger.debug( "finchClass-updateMyState, leftObstacleReading: {0} rightObstacleReading: {1}" .format(leftObstacleReading, rightObstacleReading)) # State changed or time hasn't been set if self.obstacleState[ "left"] != leftObstacleReading or self.obstacleState[ "leftStateTime"] == 0.0: finchClassLogger.info( "finchClass-updateMyState, STATE Changed, Left was:{0} is:{1}" .format(str(self.obstacleState["left"]), str(leftObstacleReading))) self.obstacleState["left"] = leftObstacleReading self.obstacleState["leftStateTime"] = stateTime self.obstacleState["leftElapsedTime"] = 0.0 else: # Calculate the elapsed time in this state self.obstacleState["leftElapsedTime"] = round( stateTime - self.obstacleState["leftStateTime"], 4) # Same as above but check the right obstacle sensor if self.obstacleState[ "right"] != rightObstacleReading or self.obstacleState[ "rightStateTime"] == 0.0: finchClassLogger.info( "finchClass-updateMyState, STATE Changed, Right was:{0} is:{1}" .format(str(self.obstacleState["right"]), str(rightObstacleReading))) self.obstacleState["right"] = rightObstacleReading self.obstacleState["rightStateTime"] = stateTime self.obstacleState["rightElapsedTime"] = 0.0 else: self.obstacleState["rightElapsedTime"] = round( stateTime - self.obstacleState["rightStateTime"], 4) # Helper to return indicator if an obstacle exists, we did this because we need the obstacle to # persist for an amount of time (thresholdInSecs) before we say it's on # Caller can specify which sensor to check (LEFT/RIGHT, if they don't then it'll return True if # either sensor reports an obstacle) def hasObstacle(self, whichOne, thresholdInSecs=OBSTACLE_PERSIST_TIME_REQUIRED): # Commented out logger... too many messages here, changed 'updateMyState' to report when state changes # finchClassLogger.debug("finchClass-hasObstacle, obstacleState:{0}".format(str(self.obstacleState))) leftObst = False if self.obstacleState["leftElapsedTime"] > thresholdInSecs: leftObst = self.obstacleState["left"] if leftObst != self.lastPersistedLeftObstacleState: finchClassLogger.info( "finchClass-hasObstacle, PERSISTED Left Obstacle State Change old/new: {0}/{1}" .format(self.lastPersistedLeftObstacleState, leftObst)) self.lastPersistedLeftObstacleState = leftObst rightObst = False if self.obstacleState["rightElapsedTime"] > thresholdInSecs: rightObst = self.obstacleState["right"] if leftObst != self.lastPersistedLeftObstacleState: finchClassLogger.info( "finchClass-hasObstacle, PERSISTED Right Obstacle State Change old/new: {0}/{1}" .format(self.lastPersistedRightObstacleState, rightObst)) self.lastPersistedRightObstacleState = rightObst # finchClassLogger.debug("finchClass-hasObstacle, leftObst:{0} rightObst{1}".format(leftObst,rightObst)) if whichOne == finchConstants.LEFT: return leftObst elif whichOne == finchConstants.RIGHT: return rightObst elif leftObst == True or rightObst == True: return True else: return False # Helper function, it returns the wheel speed taking into account what the # wheel adjustment should be, and it's polarity. def wheelHelper(self, whichWheel, logModeOnly=False): # For logging we don't want to show the polarity adjustment... could be # confusing to people analyzing wheel motion :) if logModeOnly == False: rtPol = finchConstants.RIGHTPOLARITY ltPol = finchConstants.LEFTPOLARITY else: rtPol = 1.0 ltPol = 1.0 if whichWheel == "R": if self.inWheelAdjustmentMode: return (self.rightWheel + finchConstants.RIGHTWHEELADJUSTMENT) * rtPol else: return self.rightWheel * rtPol else: if self.inWheelAdjustmentMode: return (self.leftWheel + finchConstants.LEFTWHEELADJUSTMENT) * ltPol else: return self.leftWheel * ltPol # Set the wheel speed (to move, unless both are zero) def update(self, useAdjustment): self.inWheelAdjustmentMode = useAdjustment finchClassLogger.debug( "finchClass-update, left: {0:.2f} right: {1:.2f}".format( self.wheelHelper("L"), self.wheelHelper("R"))) if (self.leftWheel != 0.0 or self.rightWheel != 0.0): # Setting wheel speed, reset the sensors self.resetState() self.myBot.wheels(self.wheelHelper("L"), self.wheelHelper("R")) else: self.myBot.wheels(0.0, 0.0) # Stop moving def stop(self): self.leftWheel = 0.0 self.rightWheel = 0.0 self.update(False) # Turn left, we do this by either increasing the speed of the right wheel # or decreasing the left wheel speed if we're already at max speed def left(self): if self.rightWheel >= finchConstants.RIGHTMAXSPEED: self.leftWheel -= finchConstants.SPEEDINCREMENT else: self.rightWheel += finchConstants.SPEEDINCREMENT self.update(self.inWheelAdjustmentMode) # Turn left for the desired degrees def leftTurn(self, degrees2Turn): self.turnTime = degrees2Turn / 360.0 self.leftWheel = finchConstants.LEFTROTATIONSPEED * -1 self.rightWheel = finchConstants.LEFTROTATIONSPEED self.update(False) time.sleep(finchConstants.LEFTROTATIONTIME * self.turnTime) self.leftWheel = 0 self.rightWheel = 0 self.update(False) # Turn right, basically increase the speed of the left wheel # or decreasing right wheel if left wheel already at max speed def right(self): if self.leftWheel >= finchConstants.LEFTMAXSPEED: self.rightWheel -= finchConstants.SPEEDINCREMENT else: self.leftWheel += finchConstants.SPEEDINCREMENT self.update(self.inWheelAdjustmentMode) # Turn right the specified number of degrees def rightTurn(self, degrees2Turn): self.turnTime = degrees2Turn / 360.0 self.leftWheel = finchConstants.RIGHTROTATIONSPEED self.rightWheel = finchConstants.RIGHTROTATIONSPEED * -1 self.update(False) time.sleep(finchConstants.RIGHTROTATIONTIME * self.turnTime) self.leftWheel = 0 self.rightWheel = 0 self.update(False) # Get the elapsed time def getElapsedTime(self): return round(time.time() - self.lasttime, 4) # Reset the elapsed timer def resetTimer(self): self.lasttime = time.time() # Run... set wheels to max speed def run(self): self.leftWheel = finchConstants.LEFTMAXSPEED self.rightWheel = finchConstants.RIGHTMAXSPEED self.update(self.inWheelAdjustmentMode) # Go faster, we determine the speed increment and increase both wheels by that amount def faster(self): increment = min(finchConstants.SPEEDINCREMENT, finchConstants.LEFTMAXSPEED - self.leftWheel, finchConstants.RIGHTMAXSPEED - self.rightWheel) self.leftWheel += increment self.rightWheel += increment self.update(self.inWheelAdjustmentMode) # Set wheels to be at certain speed def setWheels(self, lftWheel, rtWheel, adJustMode): self.leftWheel = lftWheel self.rightWheel = rtWheel self.inWheelAdjustmentMode = adJustMode self.update(self.inWheelAdjustmentMode) # Shutdown the robot def shutDown(self): self.myBot.close() # Return True if robot can move, false if there is some type of # obstacle def canMove(self, ignoreObstacles): # Add logic for other sensors # If we're going in reverse then don't check sensors if (self.leftWheel <= 0.0 and self.rightWheel <= 0.0) or ignoreObstacles: # print("canMove, ignoring obstacles") return True else: self.updateMyState() rtnValue = (self.hasObstacle("BOTH") == False) # print("canMove returns {0}".format(rtnValue)) return rtnValue # Routine when robot feels a scrap (obstacle on one side of it), pass in the side def getOutOfScrape(self, sideOfScrape): # Motion is rotate SCRAPEANGLE (if left +, right -) # Backup SCRAPEBACKUPDISTANCE # Rotate back to original angle if sideOfScrape == finchConstants.LEFT: return botUtils.calculateScrapeMovement( finchConstants.SCRAPEANGLE, finchConstants.SCRAPEBACKUPDISTANCE) else: return botUtils.calculateScrapeMovement( -finchConstants.SCRAPEANGLE, finchConstants.SCRAPEBACKUPDISTANCE) # Get out of obstacle is similar to the get out of scrap but we use a 45 degree angle and the distance to # move puts us back 1/2 of the robot's width away def getOutOfObstacle(self, directionToMove): # We want to try a position to the left or right that is 1/2 our width away # Calculate the distance we need to backup first, it's 1/2 width divided by sin(45) distanceToBackup = round( (finchConstants.TOTALWIDTH) / botUtils.degreesSin(45), 2) finchClassLogger.info( "finchClass-getOutOfObstacle, directionToMove: {0} distanceToBackup: {1}" .format(directionToMove, str(distanceToBackup))) if directionToMove == finchConstants.LEFT: # Want angle of -45 to turn right then backup return botUtils.calculateScrapeMovement(-45.0, distanceToBackup) else: return botUtils.calculateScrapeMovement(45, distanceToBackup) # Return the direction to try when you hit an obstacle, put logic in here def getObstacleDirectionToTry(self): return self.obstacleDirectionToTry def flipObstacleDirectionToTry(self): if self.obstacleDirectionToTry == finchConstants.LEFT: self.obstacleDirectionToTry = finchConstants.RIGHT else: self.obstacleDirectionToTry = finchConstants.LEFT finchClassLogger.info( "finchClass-flipObstacleDirectionToTry, new direction: {0}".format( self.obstacleDirectionToTry)) # Return the side that was last scraped, since we calculate the direction to move before # this, it's opposite out current direction... down the road see if you can derive it on # accelarator def getLastScrapeSide(self): if self.obstacleDirectionToTry == finchConstants.LEFT: return finchConstants.RIGHT else: return finchConstants.LEFT #return self.lastScrapedSide def isObstacle(self, robotPosition, robotRegionOfTravel): # Return True if you hit an obstacle (both sensors are true), if you # only have one sensor then count that as a scrape and return false # leftObst, rightObst = self.myBot.obstacle() leftObst = self.hasObstacle(finchConstants.LEFT) rightObst = self.hasObstacle(finchConstants.RIGHT) if leftObst == True and rightObst == True: return True elif leftObst == True or rightObst == True: # The robot sensor don't always report true even # when there's an obstacle right in front of it. # If the robot is close to the edge of it's region # of travel then report this as a scrape otherwise # report it as an obstacle if self.isRobotCloseToEdge(robotPosition, robotRegionOfTravel) == True: if leftObst == True: self.lastScrapedSide = finchConstants.LEFT else: self.lastScrapedSide = finchConstants.RIGHT return False else: return True return False # Determines if robot is oriented along the x or y axis (within 10 degree of it) def robotOrientedAlongAxis(self, robotPosition): orientedTowardAxis = " " if (robotPosition[botUtils.POS_OF_ANGLE] < 10 or robotPosition[botUtils.POS_OF_ANGLE] > 350): orientedTowardAxis = "X+" elif (robotPosition[botUtils.POS_OF_ANGLE] > 170 and robotPosition[botUtils.POS_OF_ANGLE] < 190): orientedTowardAxis = "X-" elif (robotPosition[botUtils.POS_OF_ANGLE] > 80 and robotPosition[botUtils.POS_OF_ANGLE] < 100): orientedTowardAxis = "Y+" elif (robotPosition[botUtils.POS_OF_ANGLE] > 260 and robotPosition[botUtils.POS_OF_ANGLE] < 280): orientedTowardAxis = "Y-" return orientedTowardAxis # Determine if robot is close to a particular regions edge def getRobotClosestEdges(self, robotPosition, regionOfTravel, threshold=THRESHOLDTOSIDE): # Calculate the distance from coordinates closeEdges = [] distanceToLeftX = robotPosition[botUtils.POS_OF_X] - regionOfTravel[0] distanceToRightX = regionOfTravel[2] - robotPosition[botUtils.POS_OF_X] distanceFromBottomY = robotPosition[ botUtils.POS_OF_Y] - regionOfTravel[1] distanceFromTopY = regionOfTravel[3] - robotPosition[botUtils.POS_OF_Y] tempString = "finchClass.py, getRobotClosesEdges, robotPosition: {0} regionOfTravel: {1} threshold: {2}" finchClassLogger.debug( tempString.format(str(robotPosition), str(regionOfTravel), threshold)) tempString = " leftXDist: {0} rightXDist: {1}, lowerYDist: {2} upperYDist: {3}" finchClassLogger.debug( tempString.format(distanceToLeftX, distanceToRightX, distanceFromBottomY, distanceFromTopY)) if (distanceToLeftX <= threshold): closeEdges.append("LX") elif (distanceToRightX <= threshold): closeEdges.append("UX") if (distanceFromBottomY <= threshold): closeEdges.append("LY") elif (distanceFromTopY <= threshold): closeEdges.append("UY") return closeEdges def getFinchReference(self): return self.myBot # Revisit this, there's definitely better way to do this... look in to matrix trasnformations def checkAndSetObstacleDirectionToTry(self, robotPosition, regionOfTravel, threshold=THRESHOLDTOSIDE): # Get closest edges finchClassLogger.debug( "finchClass.py, checkAndSetObstacleDirectionToTry, start") myCloseEdges = self.getRobotClosestEdges(robotPosition, regionOfTravel, threshold) newDirection = " " if len(myCloseEdges) > 0: finchClassLogger.debug( "finchClass.py, checkAndSetObstacleDirectionToTry, values below" ) for anEdge in myCloseEdges: finchClassLogger.debug(" {0}".format(str(anEdge))) # We are close to an edge, get the robot orientation to figure out the edges that # are used to set new direction... when in x direction we look at y values, when # pointing in y direction we look at x values robotOrientation = self.robotOrientedAlongAxis(robotPosition) if robotOrientation == "X+": if "UY" in myCloseEdges: newDirection = finchConstants.RIGHT elif "LY" in myCloseEdges: newDirection = finchConstants.LEFT elif robotOrientation == "X-": if "UY" in myCloseEdges: newDirection = finchConstants.LEFT elif "LY" in myCloseEdges: newDirection = finchConstants.RIGHT elif robotOrientation == "Y+": if "UX" in myCloseEdges: newDirection = finchConstants.LEFT elif "LX" in myCloseEdges: newDirection = finchConstants.RIGHT elif robotOrientation == "Y-": if "UX" in myCloseEdges: newDirection = finchConstants.RIGHT elif "LX" in myCloseEdges: newDirection = finchConstants.LEFT finchClassLogger.debug( "finchClass.py, checkAndSetObstacleDirectionToTry, oldDirection: {0} newDirection: {1}" .format(self.obstacleDirectionToTry, newDirection)) if newDirection != self.obstacleDirectionToTry: self.flipObstacleDirectionToTry() finchClassLogger.debug( "finchClass.py, checkAndSetObstacleDirectionToTry, start") # Helper just returns true or false stating that we're close to edge. def isRobotCloseToEdge(self, robotPosition, regionOfTravel, threshold=THRESHOLDTOSIDE): edgesCloseTo = self.getRobotClosestEdges(robotPosition, regionOfTravel, threshold) if len(edgesCloseTo) > 0: return True else: return False # Set the finches nose color :) def setLedColor(self, theColor): if theColor == finchConstants.RED: self.myBot.led(255, 0, 0) elif theColor == finchConstants.GREEN: self.myBot.led(0, 255, 0) elif theColor == finchConstants.BLUE: self.myBot.led(0, 0, 255) # Return status of all robot attributes def status(self): # This returns elapsed time since clock was set and a tuple with the attributes, the wheels, obstacle and lights # are tuples (so it's a tuple of tuples (except for temp)) leftObst, rightObst = self.myBot.obstacle() currStat = (self.getElapsedTime(), (self.wheelHelper("L", True), self.wheelHelper("R", True)), self.myBot.temperature(), (self.myBot.light()), (leftObst, rightObst), (self.myBot.acceleration())) return currStat
sleep(2) print("second line") snakyFinch.led(0, 254, 0) snakyFinch.wheels(0, 1) sleep(2) print("third line") snakyFinch.led(0, 0, 254) snakyFinch.wheels(1, 0) sleep(2) print("third line") snakyFinch.led(254, 0, 254) snakyFinch.wheels(-1, -1) sleep(2) print("third line") snakyFinch.led(0, 254, 254) snakyFinch.wheels(0.2, -1) sleep(2) print("third line") snakyFinch.led(254, 254, 254) snakyFinch.wheels(0.3, 0.3) sleep(2.5) print("third line") snakyFinch.wheels(0, 0) snakyFinch.close()
snakyFinch = Finch() # Do a six step dance snakyFinch.led(255,0,0) snakyFinch.wheels(1,1) sleep(1) snakyFinch.led(0,255,0) snakyFinch.wheels(0,1) sleep(1) snakyFinch.led(0,0,255) snakyFinch.wheels(1,0) sleep(1) snakyFinch.led(255,0,255) snakyFinch.wheels(-1,-1) sleep(0.5) snakyFinch.led(0,255,255) snakyFinch.wheels(0.2,-1) sleep(1) snakyFinch.led(255,255,255) snakyFinch.wheels(0.3,0.3) sleep(2.5) snakyFinch.close();
# Get the accelerations x, y, z, tap, shake = tweety.acceleration() # Print the acceleration data print("X is %.2f gees, Y is %.2f gees, Z is %.2f gees, tap is %r shake is %r" % (x, y, z, tap, shake)); # Use the acceleration data to set the LED: # beak up if x < -0.7 and y > -0.3 and y < 0.3 and z > -0.3 and z < 0.3: tweety.led(255,0,0); # beak down elif x > 0.7 and y > -0.3 and y < 0.3 and z > -0.3 and z < 0.3: tweety.led(0,255,0); # level elif x > -0.5 and x < 0.5 and y > -0.5 and y < 0.5 and z > 0.7: tweety.led(0,0,255); # upside down elif x > -0.5 and x < 0.5 and y > -0.5 and y < 0.5 and z < -0.7: tweety.led(0,255,255); # left wheel down elif x > -0.5 and x < 0.5 and y > 0.7 and z < 0.5 and z > -0.5: tweety.led(255,255,0); # right wheel down elif x > -0.5 and x < 0.5 and y < -0.7 and z < 0.5 and z > -0.5: tweety.led(255,0,255); # Get obstacles to use to exit loop left, right = tweety.obstacle() tweety.close()
sleep(0.2) f.wheels(0.3, 1) sleep(0.2) f.led(0, 0, 255) if i_l_light - alpha2_light > l_light or i_r_light - alpha2_light > r_light: f.led(0, 0, 0) f.wheels(0.4, 0.4) sleep(7) f.wheels(0, 0) f.led(0, 0, 0) if left_obstacle: f.led(255, 0, 0) f.wheels(-0.3, -1.0) sleep(1.0) elif right_obstacle: f.led(255, 255, 0) f.wheels(-1.0, -0.3) sleep(1.0) else: f.wheels(1.0, 1.0) f.led(0, 255, 0) zAccel = f.acceleration()[2] f.close()
# out the test codes in all of the import python files sket = dataSocketSettings() #Instantiate the Finch object autopilotFinch = Finch() k = 0 while True: # just testing for 50 samples if (k == 50): break # receive ultrasonic range finder data from the raspberry pi d = dataReceiveSocket(sket, 1) # define the left, right wheel speeds leftWheel = d[0] rightWheel = d[1] # Before you send the instructions to the wheels, make sure that # they are converted into 0.1 -> 1.0 values # Send instructions to the Finch Wheels autopilotFinch.wheels(leftWheel, rightWheel) # increment k = k + 1 autopilotFinch.close() sket.close()