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)