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!
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]
