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()
'your answer.') # Create the variables we will use to track our state across multiple # executions of the loop: # Remember if we were upside down the last time the loop ran or not. We # want to print our answer the first moment we detect that the user # flipped us upside down. Then, we don't want to print another answer # until we can tell that the user has flipped us right-side up and then # upside down again. was_upside_down_before = False # Track if we've encountered an obstacle (like if the user waves a hand # in front of the beak). Note we are also initializing them with real # data with a call to the obstacle function. left_obstacle, right_obstacle = tweety.obstacle() while not left_obstacle and not right_obstacle: # Get our current acceleration status. x, y, z, tap, shake = tweety.acceleration() if x > -0.5 and x < 0.5 and y > -0.5 and y < 0.5 and z < -0.7: # Based on x, y and z, we're now upside down. # Green LED (hints to the user that we're upside down and ready # to print an answer). tweety.led(0, 255, 0) if not was_upside_down_before: # If we weren't upside down already, that means the user # *just* flipped us upside down, so take action!
while laps <= 0: laps = int(input('Enter number of laps: ')) if laps < 0: print('Cannot swim a negative number of laps!') elif laps == 0: print('Zero laps? I want to swim!') # Move forward until an obstacle is present and measure the time: 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!
from finch import Finch from time import sleep from random import randint tweety = Finch() left, right = tweety.obstacle() while not left and not right: x, y, z, tap, shake = tweety.acceleration() print("X : ", x) print("Y : ", y) print("Z : ", z) left, right = tweety.obstacle() sleep(.5) tweety.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) self.myLights.readValues() # 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) # [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 ) # # [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 ) # def isLight(self): current_left, current_right = self.tweety.light() return current_left, current_right def scurryTowardsLights(self): lspeed = 0.3 rspeed = 0.5 clspeed = 0.1 crspeed = 0.5 onCurve = 0 onMax = 0 maxleft, maxright = self.myLights.getMax() tmpmaxleft = maxleft + .25 tmpmaxright = maxright + .25 if (tmpmaxleft) > .75: tmpmaxleft = .75 if (tmpmaxright) > .75: tmpmaxright = .75 while (True): # Check lights, then obstacles left_light, right_light = self.myLights.lightStatus() self.left_obst, self.right_obst = self.tweety.obstacle() if (self.checkForObstacle()): #switch the speeds tmp = lspeed ctmp = clspeed lspeed = rspeed rspeed = tmp clspeed = crspeed clspeed = ctmp if left_light == 0 and right_light == 0: # Just keep swimming print("Just keep swimming") self.delay(0.3, lspeed, rspeed) onCurve += 1 if onCurve == 10: print("Small curve") self.setWheels(0.0, 0.0) self.delay(3.0, clspeed, crspeed) self.setWheels(lspeed, rspeed) # Natural curve speed (testing) onCurve = 0 elif left_light > 0 or right_light > 0: onCurve = 0 # Find "brightest" # stop ... move towards the light self.setWheels(0.0, 0.0) leftval, rightval = self.myLights.getComparison() if leftval > rightval: # Turn towards our left print("Going left") self.turnLeft() else: print("Going right") # Turn towards our right self.turnRight() if ((leftval + maxleft) > (tmpmaxleft)) and ((rightval + maxright) > tmpmaxright): print("Under bright area!") while (True): self.setWheels(0.0, 0.0) self.delay(0.3, 0.5, 0.5) self.setWheels(0.0, 0.0) left, right = self.isLight() print(left, " ", right) print(tmpmaxleft, " ", tmpmaxright) if (left > tmpmaxleft) or (right > tmpmaxright): tmpmaxleft = left tmpmaxright = right sleep(0.5) else: self.delay(0.3, -0.5, -0.5) self.setWheels(0.0, 0.0) break os._exit(0) elif right_light < 0 or right_light < 0: onCurve = 0 print("got darker") def checkForObstacle(self): if self.left_obst == True and self.right_obst == True: print("Obstacle straight ahead") self.setWheels(-(self.left_wheel), -(self.right_wheel)) sleep(0.5) self.setWheels(0.0, 0.0) sleep(0.1) self.setWheels(0.0, 0.5) sleep(0.5) return True elif self.right_obst == True: print("Obstacle on right") self.setWheels(-(self.left_wheel), -(self.right_wheel)) sleep(0.5) self.setWheels(0.0, 0.0) sleep(0.1) self.setWheels(0.5, 0.0) sleep(0.5) return True elif self.left_obst == True: print("Obstacle on left") self.setWheels(-(self.left_wheel), -(self.right_wheel)) sleep(0.5) self.setWheels(0.0, 0.0) sleep(0.1) self.setWheels(0.5, 0.0) sleep(0.5) return True else: print("No obstacle") return False return False def turnRight(self): self.setWheels(.5, .25) def turnLeft(self): self.setWheels(0.25, .5) def straight(self): self.setWheels(0.4, 0.4) def charging(self): self.tweety.led("#00FF00") self.tweety.buzzer(1.0, 800) def discharging(self): self.tweety.led("#FF0000") def delay(self, time, lspeed, rspeed): timer = 0 while (timer < time): self.setWheels(lspeed, rspeed) if (self.checkForObstacle() == True): tmp = lspeed lspeed = rspeed rspeed = tmp self.setWheels(lspeed, rspeed) continue else: timer = timer + 0.1 sleep(0.1) continue
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)
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 def __init__(self): self.tweety = Finch() self.left_wheel = 0.0 self.right_wheel = 0.0 self.tweety.wheels(self.left_wheel, self.right_wheel) self.left_obst, self.right_obst = self.tweety.obstacle() # [FUNCTION]Name - accelerate( speed ) # Synopsis - # def accelerate ( speed ) : # speed, an integer value between (-1) and 1 to accelerate the finch # def changeSpeed ( self, new_speed ): old_speed = self.left_wheel if new_speed == old_speed: print("Same speed") return # Decelerate if old_speed > new_speed: self.__decelerate(2, new_speed) # Accelerate else: self.__accelerate(2, new_speed) #set wheels as last action in case error in accuracy self.setWheels( new_speed, new_speed ) return # [FUNCTION]Name - changeBoth ( left, right ) # Synopsis - # def changeBoth ( left, right ) : # left, an integer value between (-1) and 1 to accelerate the finch # right, an integer value between (-1) and 1 to accelerate the finch # def changeBoth ( self, left, right ) : if left == right: changeSpeed(left) return elif self.left_wheel != left: if left > self.left_wheel: self.__accelerate( 0, left ) else: self.__decelerate( 0, left ) elif self.right_wheel != right: if right > self.right_wheel: self.__accelerate( 1, right ) else: self.__decelerate( 1, right ) else: wheel = 2 return 0 # [PRIV. FUNCTION]Name - __accelerate ( self, wheel, new_speed ) # Synopsis - # def __accelerate ( self, wheel, new_speed ) : # wheel, which wheel[0 for left, 1 for right] or both[any other integer] to change speed for # new_speed, a double value to change the speed to between -1 and 1 def __accelerate ( self, wheel, new_speed ): if ( new_speed > 1.0 or new_speed < -1.0): print ("Invalid Speed") return # Change left wheel speed @ wheel == 0 if wheel == 0: old_speed = self.left_wheel while old_speed < new_speed: self.setWheels ( old_speed, self.right_wheel ) old_speed = old_speed + 0.1 sleep(0.15) self.setWheels (new_speed, self.right_wheel) #Change right wheel speed @ wheel == 1 elif wheel == 1: old_speed = self.right_wheel while old_speed < new_speed: self.setWheels ( self.left_wheel, old_speed ) old_speed = old_speed + 0.1 sleep(0.15) self.setWheels (self.left_wheel, new_speed) # Change speed of both wheels else: old_speed = self.right_wheel while old_speed < new_speed: self.setWheels ( old_speed, old_speed ) old_speed = old_speed + 0.1 sleep(0.15) self.setWheels (new_speed, new_speed) return # [PRIV. FUNCTION]Name - decelerate ( self, wheel, new_speed ) # Synopsis - # def __decelerate ( self, wheel, new_speed ): # wheel, which wheel[0 for left, 1 for right] or both[any other integer] to change speed for # new_speed, a double value to change the speed to between -1 and 1 def __decelerate ( self, wheel, new_speed ): if ( new_speed > 1.0 or new_speed < -1.0): print ("Invalid Speed") return # Change left wheel speed @ wheel == 0 if wheel == 0: old_speed = self.left_wheel while old_speed > new_speed: self.setWheels ( old_speed, self.right_wheel ) old_speed = old_speed - 0.1 sleep(0.15) self.setWheels (new_speed, self.right_wheel) return #Change right wheel speed @ wheel == 1 elif wheel == 1: old_speed = self.right_wheel while old_speed > new_speed: self.setWheels ( self.left_wheel, old_speed ) old_speed = old_speed - 0.1 sleep(0.15) self.setWheels(self.left_wheel, new_speed) return #Change both wheel speed @ wheel == not 1 or 0 else: old_speed = self.right_wheel while old_speed > new_speed: self.setWheels ( old_speed, old_speed ) old_speed = old_speed - 0.1 sleep(0.15) self.setWheels(new_speed, new_speed) return return 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 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
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
# A simple program that changes the Finch LED based on orientation. from finch import Finch from random import randint # Instantiate the Finch object and connect to Finch tweety = Finch() left, right = tweety.obstacle() # Do the following while no obstacles are detected by Finch while not left and not right: # 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);
from time import sleep f = Finch() zAccel = f.acceleration()[2] i_temp = f.temperature() i_l_light, i_r_light = f.light() alpha_light = 0.20 alpha2_light = 0.25 alpha_temp = 0.65 while zAccel > -0.7: left_obstacle, right_obstacle = f.obstacle() l_light, r_light = f.light() temp = f.temperature() print(temp) if temp > i_temp + alpha_temp: k = 1 while k < 11: f.led(141, 21, 165) f.wheels(-1, -1) sleep(0.2) f.led(255, 255, 255) f.wheels(1, 1) sleep(0.2) f.wheels(1, 1)
from finch import Finch import time import os screen = curses.initscr() screen.refresh() curses.noecho() screen.keypad(1) curses.halfdelay(1) screen.addstr("Use WASD or arrow keys to control bot. Use X to exit controls.") finch = Finch() left, right = finch.obstacle() done = False while not done: key = screen.getch() print(key) rightWheel = 0.0 leftWheel = 0.0 if key == 119 or key == curses.KEY_UP: rightWheel = 0.5 leftWheel = 0.5 elif key == 115 or key == curses.KEY_DOWN: rightWheel = -0.5
# A simple program that wanders and avoids obstacles from finch import Finch from time import sleep # Instantiate the Finch object and connect to Finch tweety = Finch() # Get the Z-Axis acceleration zAccel = tweety.acceleration()[2] # Do the following while the Finch is not upside down (z value in gees above -0.7) while zAccel > -0.7: left_obstacle, right_obstacle = tweety.obstacle() # If there's an obstacle on the left, back up and arc if left_obstacle: tweety.led(255,0,0) tweety.wheels(-0.3,-1.0) sleep(1.0) # Back up and arc in the opposite direction if there's something on the right elif right_obstacle: tweety.led(255,255,0) tweety.wheels(-1.0, -0.3) sleep(1.0) # Else just go straight else: tweety.wheels(1.0, 1.0) tweety.led(0,255,0) # Keep reading in the Z acceleration zAccel = tweety.acceleration()[2]