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()
"""Let's make the Finch robot dance!""" from finch import Finch from time import sleep finch = Finch() ##### CHANGE CODE BELOW THIS LINE ##### finch.led(0, 255, 0) finch.wheels(0.75, 0.75) sleep(1.5) finch.led(0, 0, 255) finch.wheels(-0.75, -0.75) sleep(1.5) finch.halt()
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
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
import ultrasonicRangeRpi
from finch import Finch
from time import sleep
#Instantiate the Finch Object
TestFinch = Finch()
#Details about the Finch
FinchWidth = 20 #Distance between mid points of two tires
TireCircumf = 20.797 #Circumference of Finch Wheels
l, ril, rel = ultrasonicRangeRpi.lists()
#Starting Speed (0.5 on each wheel)
TestFinch.wheels(0.5, 0.5)
while True:
# Find the distances from each sensor
sensor1 = ultrasonicRangeRpi.ultrasound(26, 19, 3) #Left Ultrasound
sensor2 = ultrasonicRangeRpi.ultrasound(20, 21, 2) #Rear Ultrasound
sensor3 = ultrasonicRangeRpi.ultrasound(23, 24, 1) #Right Ultrasound
# Enter a number between 0-1, with 0.1 increments (example: 0.2, 0.5, 0.6, 0.9 etc.)
rw, lw = ultrasonicRangeRpi.finchSpeed(0.5, 0.5)
ultrasonicRangeRpi.listGenerator(sensor1, sensor2, sensor3, l, rel, ril)
if ((len(l) == 2) and (len(ril) == 2)):
changeR, changeL, changeRear = ultrasonicRangeRpi.maneuver(
from finch import Finch from time import sleep import notes #Main function for the music player example program""" #Initialize the finch finch = Finch() finch.wheels(1.0, 1.0)
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();
# A simple Finch dance in Python
from finch import Finch
from time import sleep
print("Finch's First Python program.")
# Instantiate the Finch object
snakyFinch = Finch()
# Do a six step dance
snakyFinch.led(254, 0, 0)
snakyFinch.wheels(1, 1)
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")
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
leftWheel = -0.5
elif key == 100 or key == curses.KEY_RIGHT:
rightWheel = -0.5
leftWheel = 0.5
elif key == 97 or key == curses.KEY_LEFT:
rightWheel = 0.5
leftWheel = -0.5
elif key == 120:
done = True
elif key == -1:
rightWheel = 0.0
leftWheel = 0.0
finch.wheels(rightWheel, leftWheel)
left, right = finch.obstacle()
screen.refresh()
finch.halt()
finch.close()
curses.endwin()
os.system('stty sane')
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]
tweety.close()
import curses
from finch import Finch
f = Finch()
stdscr = curses.initscr()
curses.cbreak()
stdscr.keypad(1)
stdscr.addstr(0, 10, "Hit 'q' to quit")
stdscr.refresh()
direction = 0
key = ''
while key != ord('q'):
key = stdscr.getch()
stdscr.addch(20, 25, key)
stdscr.refresh()
if key == curses.KEY_UP:
direction = min(direction + 1, 1)
stdscr.addstr(2, 20, "Up")
f.wheels(direction, direction)
elif key == curses.KEY_DOWN:
direction = max(direction - 1, -1)
stdscr.addstr(3, 20, "Down")
f.wheels(direction, direction)
def foward():
robot.wheels(0.5, 0.5)
pass
def backward():
pass
def left():
pass
def right():
pass
def arcleft():
robot.wheels(1, 0)
sleep(2)
def arcright():
robot.wheels(0, 1)
sleep(2)
robot.wheels(-1, -1)
sleep(7)
# Finch Racetrack Program # authors: Juhi, Simon, Andrew from finch import Finch from time import sleep, time finch = Finch() finch.led(255, 0, 0) finch.wheels(0.5, 0.57) sleep(2.5)
from finch import Finch
from time import sleep
from random import randint
tweety = Finch()
accList = []
left, right = tweety.obstacle()
#tweety.wheels(.94, 1)
tweety.wheels(.46, .5)
while 1:
#while 1:
x, y, z, tap, shake = tweety.acceleration()
acc = [x, y, z]
accList.append(acc)
# print ("X : ", x)
# print ("Y : ", y)
# print ("Z : ", z)
left, right = tweety.obstacle()
#sleep(.5)
#stop tweety from moving
tweety.wheels(0.0, 0.0)
print(accList)
tweety.close()
laps = 0
# Get the number of laps Finch will swim:
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
# A simple Finch dance in Python
from finch import Finch
from time import sleep
print("Finch's First Python program.")
# Instantiate the Finch object
snakyFinch = Finch()
# Do a dance.. think of a wheelchair
snakyFinch.buzzer(1.5, 450)
sleep(1.0)
snakyFinch.led(0, 255, 0)
snakyFinch.wheels(0, 1)
sleep(3.05)
snakyFinch.led(0, 255, 255)
snakyFinch.wheels(0.2, -0.2)
sleep(1)
snakyFinch.led(255, 0, 0)
snakyFinch.wheels(1, 1) #moving forward
sleep(1.5)
snakyFinch.led(0, 255, 0)
snakyFinch.wheels(0, 1) #more spinning
sleep(1)
snakyFinch.led(0, 0, 255)
snakyFinch.wheels(1, 0) #spinning but in the other directions
sleep(1)
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)
k = k + 1
i_temp = f.temperature()
if i_l_light + alpha_light <= l_light or i_r_light + alpha_light <= r_light:
f.led(0, 0, 255)
f.wheels(-1, -0.3)
sleep(0.2)
f.wheels(0.3, 1)
sleep(0.2)
f.led(0, 0, 255)
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();
# testfinchfunctions, an example program for the Finch
# Tests all parts of the Finch: prints all sensor values, changes the led
# moves the wheels and beeps the buzzer.
from time import sleep
from finch import Finch
finch = Finch()
print('Temperature %5.2f' % finch.temperature())
print()
finch.wheels(1.0, -1.0)
sleep(0.5)
finch.wheels(0.0, 0.0)
for count in range(5):
finch.led(0, 50 * count, 0)
x, y, z, tap, shake = finch.acceleration()
print('Acceleration %5.3f, %5.3f %5.3f %s %s' % (x, y, z, tap, shake))
left_light, right_light = finch.light()
print('Lights %5.3f, %5.3f' % (left_light, right_light))
left_obstacle, right_obstacle = finch.obstacle()
print('Obstacles %s, %s' % (left_obstacle, right_obstacle))
print()
finch.buzzer(0.8, 100 * count)
sleep(1)
finch.led('#FF0000')
finch.buzzer(5, 440)
sleep(5)
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();
from finch import Finch
from time import sleep
from random import randint
tweety = Finch()
left, right = tweety.obstacle()
x, y, z, tap, shake = tweety.acceleration()
tweety.wheels(.94, 1.0)
sleep(1.4)
while not left and not right:
if tap:
print("Tap!")
tweety.wheels(0.0, 0.0)
sleep(2)
tweety.wheels(.94, 1.0)
sleep(1.4)
x, y, z, tap, shake = tweety.acceleration()
left, right = tweety.obstacle()
tweety.wheels(0.0, 0.0)
tweety.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
finch = Finch()
fp = open("mid.txt", "r")
line = fp.readline()
while line:
line.strip()
if line.startswith("#"):
print("comment")
line = fp.readline()
continue
left = random.uniform(-0.5, 0.5)
right = random.uniform(-0.5, 0.5)
print("L:" + str(left), "R:" + str(right))
finch.wheels(left, right)
red = random.randint(0, 255)
green = random.randint(0, 255)
blue = random.randint(0, 255)
print(red, green, blue)
finch.led(red, green, blue)
line.replace(" ", "")
things = line.split(",")
if things[0] == "T":
finch.buzzer_with_delay(
int(float(things[2])) / 1000, int(float(things[1])))
if things[0] == "D": sleep(int(float(things[1])) / 1000)
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)
#tweety.led(255,255,0)
#input()
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(-1.0, -0.3)
sleep(1.0)
print("Uh oh! looks like we've found an obstical!")
# Else just go straight
# 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)
print("Uh oh! looks like we've found an obstical!")
# Else just go straight
# Race track driver, an example program for the Finch # Watch the Finch navigate a square race track from finch import Finch from time import sleep #Main function for the race track driver example program.""" #Initialize the finch finch = Finch() #Set both wheels to one-half forward throttle for 1.5s finch.wheels(0.5, 0.5) finch.led(0, 255, 255) sleep(1.5) # Now set the left wheel at half throttle, right wheel off to turn finch.wheels(0.5, 0) finch.led(0, 255, 0) sleep(1.28) finch.wheels(0.5, 0.5) finch.led(0, 255, 255) sleep(1.5) finch.wheels(0.5, 0) finch.led(0, 255, 0) sleep(1.28) finch.wheels(0.5, 0.5) finch.led(0, 255, 255)
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)
# A simple Finch dance in Python
from finch import Finch
from time import sleep
print("Finch's First Python program.")
# Instantiate the Finch object
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)
#Finch Maze Program #authors: Simon, Juhi, Andrew from finch import Finch from time import sleep, time finch = Finch() finch.led(255, 0, 0) # Turns light red start = time() #Makes Finch drive through track and then celebrate at the end finch.wheels(.8, .87) sleep(5.5) finch.wheels(0, 1) sleep(.6) finch.led(225, 0, 225) finch.wheels(.8, .83) sleep(2.5) finch.wheels(1, 0) sleep(.35) finch.led(0, 150, 150) finch.wheels(.8, .81) sleep(8) finch.wheels(0, 0) finch.wheels(-1,1) sleep(3) finch.wheels(1, -1) sleep(3) finch.wheels(0,0)
left_wheel = 0
break
elif finchy == "d":
if right_wheel + left_wheel <= 1:
left_wheel = left_wheel + 0.2
else:
right_wheel = right_wheel - 0.2
elif finchy == "a":
if right_wheel + left_wheel <= 1:
right_wheel = right_wheel + 0.2
else:
left_wheel = left_wheel - 0.2
elif finchy == "s":
left_wheel = 0
right_wheel = 0
elif finchy == "red":
led_red = input("How much?")
elif finchy == "green":
led_green = input("How much?")
elif finchy == "blue":
led_blue = input("How much?")
elif finchy == "temp":
print(str(finch.temperature()) + " degrees celcius")
finch.wheels(left_wheel, right_wheel)
finch.led(led_red, led_green, led_blue)
finch.close()
# Race track driver, an example program for the Finch
# Watch the Finch navigate a square race track
from finch import Finch
from time import sleep
#Main function for the race track driver example program."""
#Initialize the finch
finch = Finch()
#Set both wheels to one-half forward throttle for 1.5s
finch.wheels(0.5,0.5)
finch.led(0, 255, 255)
sleep(1.5)
# Now set the left wheel at half throttle, right wheel off to turn
finch.wheels(0.5,0)
finch.led(0, 255, 0)
sleep(1.28)
finch.wheels(0.5,0.5)
finch.led(0, 255, 255)
sleep(1.5)
finch.wheels(0.5,0)
finch.led(0, 255, 0)
sleep(1.28)
# testfinchfunctions, an example program for the Finch
# Tests all parts of the Finch: prints all sensor values, changes the led
# moves the wheels and beeps the buzzer.
from time import sleep
from finch import Finch
finch = Finch()
print('Temperature %5.2f' % finch.temperature())
print()
finch.wheels(1.0, -1.0)
sleep(0.5)
finch.wheels(0.0, 0.0)
for count in range(5):
finch.led(0, 50*count, 0)
x, y, z, tap, shake = finch.acceleration()
print ('Acceleration %5.3f, %5.3f %5.3f %s %s' %
(x, y, z, tap, shake))
left_light, right_light = finch.light()
print ('Lights %5.3f, %5.3f' % (left_light, right_light))
left_obstacle, right_obstacle = finch.obstacle()
print('Obstacles %s, %s' % (left_obstacle, right_obstacle))
print()
finch.buzzer(0.8, 100*count)
sleep(1)
finch.led('#FF0000')
finch.buzzer(5, 440)
laps = 0
# Get the number of laps Finch will swim:
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
# 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()
