def botgalToFrisbee():
print("botgaltofrisbee")
link.enable_servo(c.grabberArm)
move.cubeHolderArmMesa()
drive.withStop(200, 200, 2.4)
move.grabberOpen()
t.sleep (1.000)
move.grabberArmStraightUp()
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 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 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 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 grabFrisbee():
link.enable_servo(c.grabber)
link.enable_servo(c.frisbeeGrabber)
move.grabberOpen()
move.frisbeeGrabberOpen()
#t.sleep(1.00)
move.grabberArmGrabFrisbee( 40 )
t.sleep(1.00)
move.grabberArmFinal(10)
t.sleep(1.00)
move.grabberMidClose()
#t.sleep(1.00)
move.frisbeeGrabberClose()
t.sleep(1.00)
move.grabberSlowOpen()
move.grabberArmBack(5)
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