def testGrabberArm():
print "testing grabberArm"
link.set_servo_position(c.grabberArm, c.grabberArmClear)
link.enable_servo(c.grabberArm)
grabberArmStraightUp()
grabberArmClear()
link.disable_servo(c.grabberArm)
link.disable_servo(c.cubeHolderArm)
def testFrisbeeGrabber():
print "test frisbeeGrabber"
link.set_servo_position(c.frisbeeGrabber, c.frisbeeGrabberClose)
link.enable_servo(c.frisbeeGrabber)
frisbeeGrabberOpen()
t.sleep(.5)
frisbeeGrabberClose(10)
link.disable_servo(c.frisbeeGrabber)
def init() :
print "Turn on Create..."
link.create_connect()
link.create_full()
link.set_servo_position(c.armPort, c.armUp)
link.set_servo_position(c.clawPort, c.clawOpen)
link.enable_servo(c.armPort)
link.enable_servo(c.clawPort)
print "...Ready!"
def moveServo( servo, endPos, speed=10 ) :
now = link.get_servo_position( servo )
if now > endPos:
speed = -speed
for i in range (now, endPos, speed ):
link.set_servo_position( servo, i)
sleep(0.010)
link.set_servo_position( servo, endPos )
sleep( 0.010 )
def testGrabber():
print "testing grabber"
link.set_servo_position(c.grabber, c.grabberClearClosed)
link.enable_servo(c.grabber)
t.sleep(.5)
grabberOpenInit()
t.sleep(1) #was.4
grabberClearClosed()
t.sleep(.5)
link.disable_servo(c.grabber)
def testCubeHolderArm():
print "testing cubeHolderArm"
link.set_servo_position(c.cubeHolderArm, c.cubeHolderArmCompleteDown)
link.enable_servo(c.cubeHolderArm)
cubeHolderArmSlightBack()
t.sleep(.5)
testETScanner
cubeHolderArmCompleteDown()
t.sleep(.5)
link.disable_servo(c.cubeHolderArm)
def moveServo(servo, endPos, speed=10):
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = link.get_servo_position(servo)
if now > endPos:
speed = -speed
for i in range(now, endPos, speed):
link.set_servo_position(servo, i)
time.sleep(0.010)
link.set_servo_position(servo, endPos)
time.sleep(0.010)
def moveFrisbeeGrabber(endPos, speed=10):
now = link.get_servo_position(c.frisbeeGrabber)
if now > endPos:
speed = -speed
#if
for i in range(now, endPos, speed):
link.set_servo_position(c.frisbeeGrabber, i)
t.sleep(0.010)
#for
link.set_servo_position(c.frisbeeGrabber, endPos)
t.sleep(0.010)
def movegrabberArm(endPos, speed=10):
now = link.get_servo_position(c.grabberArm)
if now > endPos:
speed = -speed
#if
for i in range(now, endPos, speed):
link.set_servo_position(c.grabberArm, i)
t.sleep(0.010)
#for
link.set_servo_position(c.grabberArm, endPos)
t.sleep(0.010)
def grabCubes():
move.cubeHolderArmUp()
move.grabberArmDrop()
move.grabberOpen()
drive.withStop(100, 100, 4)
drive.withStop(-100, 100, 2.00)
drive.withStop(100, 100, 2.5)
move.grabberArmDown()
t.sleep(2)
link.set_servo_position( c.grabber, 400)
'''move.closeGrabber()'''
t.sleep(2)
move.grabberArmDrop()
drive.withStop(50, 50, 1.5)
def moveServo( servo, endPos, speed=10 ) :
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = link.get_servo_position( servo )
if now > 2048 :
PROGRAMMER_ERROR( "Servo setting too large" )
if now < 0 :
PROGRAMMER_ERROR( "Servo setting too small" )
if now > endPos:
speed = -speed
for i in range (now, endPos, speed ):
link.set_servo_position( servo, i)
time.sleep(0.010)
link.set_servo_position( servo, endPos )
time.sleep( 0.010 )
def moveServo(servo, endPos, speed=10):
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = link.get_servo_position(servo)
if now > 2048:
PROGRAMMER_ERROR("Servo setting too large")
if now < 0:
PROGRAMMER_ERROR("Servo setting too small")
if now > endPos:
speed = -speed
for i in range(now, endPos, speed):
link.set_servo_position(servo, i)
time.sleep(0.010)
link.set_servo_position(servo, endPos)
time.sleep(0.010)
def testServo():
print "testing clapper"
link.set_servo_position(c.clapper, c.clapperTight)
link.enable_servo(c.clapper)
time.sleep(1.0)
link.set_servo_position(c.clapper, c.clapperWide)
time.sleep(1.0)
print "testing sorter"
link.set_servo_position(c.sorter, c.sorterLeft)
link.enable_servo(c.sorter)
time.sleep(1.0)
link.set_servo_position(c.sorter, c.sorterRight)
time.sleep(1.0)
link.set_servo_position(c.sorter, c.sorterCenter)
time.sleep(1.0)
print "testing kicker"
link.set_servo_position(c.kicker, c.kickerOut)
link.enable_servo(c.kicker)
time.sleep(1.0)
link.set_servo_position(c.kicker, c.kickerReady)
time.sleep(1.0)
def testServo():
print "testing clapper"
link.set_servo_position(c.clapper, c.clapperTight)
link.enable_servo(c.clapper)
time.sleep(1.0)
link.set_servo_position(c.clapper, c.clapperWide)
time.sleep(1.0)
print "testing sorter"
link.set_servo_position(c.sorter, c.sorterLeft)
link.enable_servo(c.sorter)
time.sleep(1.0)
link.set_servo_position(c.sorter, c.sorterRight)
time.sleep(1.0)
link.set_servo_position(c.sorter, c.sorterCenter)
time.sleep(1.0)
print "testing kicker"
link.set_servo_position(c.kicker, c.kickerOut)
link.enable_servo(c.kicker)
time.sleep(1.0)
link.set_servo_position(c.kicker, c.kickerReady)
time.sleep(1.0)
def backAwayFromBin():
print "back Away From Bin"
link.set_servo_position(c.grabberArm, c.grabberArmRelease)
link.enable_servo(c.grabberArm)
move.cubeHolderArmMid(20)
drive.withStop(-200, -200, 1.625)#3.25
def grabberOpen():
link.set_servo_position(c.grabber, c.grabberOpen)
def grabberClose():
link.set_servo_position(c.grabber, c.grabberClosed)