def moveToFrisbee():
#t.sleep(2)
drive.noStop(-100, -100, 0)
while ( link.analog_et(c.ETport) < 425):
pass
print link.analog_et(c.ETport)
drive.withStop(0, 0, 0)
def dumpPod():
print "dump pod"
move.grabberArmDown(20)
move.grabberOpen()
t.sleep (.50)
move.grabberArmRelease(15)
drive.withStop( -250, 250, 0.75 )
def driveToBlock():
print "driveToBlock"
move.cubeHolderArmBackMesa()
if c.isPrime:
drive.withStop( 100, 100, 1.5 )
else:
drive.withStop( 100, 100, 1.6)
def sortTribble() :
print "sortTribble"
link.camera_update()
time.sleep(.15)
link.camera_update()
print "camera updated"
found = 1
if link.get_object_area( c.chanGreen, 0 ) >= c.blobSize:
print "green, y=", link.get_object_center_y(c.chanGreen, 0)
if link.get_object_center_y(c.chanGreen, 0) >= 90:
drive.withStop(30, 30, .5)
link.camera_update()
print "*******new y=", link.get_object_center_y(c.chanGreen, 0)
#act.DEBUG("bad green alignment")\
print link.get_object_area( c.chanGreen, 0 )
print "moveSorter"
sortAndKick( c.sorterLeft)
elif link.get_object_area(c.chanRed, 0 ) >= c.blobSize or link.get_object_area(c.chanGold, 0 ) >= c.blobSize:
print "red/gold"
print "red, y=", link.get_object_center_y(c.chanRed, 0)
if link.get_object_center_y(c.chanRed, 0) >= 90:
drive.withStop(30, 30, .5)
link.camera_update()
print "*******new y=", link.get_object_center_y(c.chanRed, 0)
print link.get_object_area( c.chanRed, 0 )
print "moveSorter"
sortAndKick( c.sorterRight)
else:
print "nothing found"
found = 0
return found
def sortTribble():
print "sortTribble"
link.camera_update()
time.sleep(.15)
link.camera_update()
print "camera updated"
found = 1
if link.get_object_area(c.chanGreen, 0) >= c.blobSize:
print "green, y=", link.get_object_center_y(c.chanGreen, 0)
if link.get_object_center_y(c.chanGreen, 0) >= 90:
drive.withStop(30, 30, .5)
link.camera_update()
print "*******new y=", link.get_object_center_y(c.chanGreen, 0)
#act.DEBUG("bad green alignment")\
print link.get_object_area(c.chanGreen, 0)
print "moveSorter"
sortAndKick(c.sorterLeft)
elif link.get_object_area(c.chanRed,
0) >= c.blobSize or link.get_object_area(
c.chanGold, 0) >= c.blobSize:
print "red/gold"
print "red, y=", link.get_object_center_y(c.chanRed, 0)
if link.get_object_center_y(c.chanRed, 0) >= 90:
drive.withStop(30, 30, .5)
link.camera_update()
print "*******new y=", link.get_object_center_y(c.chanRed, 0)
print link.get_object_area(c.chanRed, 0)
print "moveSorter"
sortAndKick(c.sorterRight)
else:
print "nothing found"
found = 0
return found
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 turnToMesa():
print "turnToMesa"
if c.isPrime:
drive.withStop( -250, 250, 0.73 ) #was -250, 250, 0.7550 #correct code
else:
drive.withStop( -250, 250, 0.755 ) #was 0.750
move.grabberArmDown()
'''
def sortAndKick(direction):
servo.moveSorter( direction, 100 )
drive.withStop(-50, -50, 0.1)
time.sleep(.2)
print "moveKicker"
servo.moveKicker( c.kickerOut, 2000 )
time.sleep(0.4) #0.5
servo.moveSorter( c.sorterCenter ,200)
servo.moveKicker( c.kickerReady, 2000 )
time.sleep(0.1)#was .5
def wiggle():
moveSorter(c.sorterRightish, 100)
moveSorter(c.sorterLeftish, 100)
moveSorter(c.sorterCenter, 100)
moveClapper(c.clapperDrive, 200)
drive.withStop(-50, 25, .5)
moveClapper(c.clapperOpen, 150) #200 #was closed
drive.withStop(50, -25, .65)
drive.noStop(55, 50, .3)
moveClapper(c.clapperClosed)
def wiggle():
moveSorter(c.sorterRightish, 100)
moveSorter(c.sorterLeftish, 100)
moveSorter(c.sorterCenter, 100)
moveClapper(c.clapperDrive, 200)
drive.withStop(-50, 25, .5)
moveClapper(c.clapperOpen, 150) #200 #was closed
drive.withStop(50, -25, .65)
drive.noStop( 55, 50, .3)
moveClapper(c.clapperClosed)
def sortAndKick(direction):
servo.moveSorter(direction, 100)
drive.withStop(-50, -50, 0.1)
time.sleep(.2)
print "moveKicker"
servo.moveKicker(c.kickerOut, 2000)
time.sleep(0.4) #0.5
servo.moveSorter(c.sorterCenter, 200)
servo.moveKicker(c.kickerReady, 2000)
time.sleep(0.1) #was .5
def podToFrisbee():
move.cubeHolderArmParallel()
move.grabberArmRelease(15)
if c.isPrime:
drive.withStop(-400, -400, 1.70 )#200,200,3.5
else:
drive.withStop(-400, -400, 1.625 ) #200,200,3.25
move.grabberArmStraightUp()
#move.cubeHolderArmUp(10)
#drive.withStop(-200, -200, 0.55)
drive.withStop( -250, 250, 0.80 ) #was 0.65
drive.withStop(-200, -200, 0.50)
move.cubeHolderArmMesa()
t.sleep(8.50)#9.00
#square up on outside wall
drive.withStop(200, 200, 2.90)#1.8
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 driveToMesa():
print "driveToMesa"
move.cubeHolderArmSlightBack()
drive.noStop( 200, 200, 1.0 ) #100,100,2.00
if c.isPrime:
drive.withStop( 200, 200, 3.4) #was 3.3
else:
drive.withStop(200, 200, 3.5)
drive.noStop(-50, -50, 0)
while ( link.analog_et(c.ETport) > 350):
pass
print link.analog_et(c.ETport)
if c.isPrime:
drive.withStop(-75, -75, 0.50)
else:
drive.withStop(-50, -50, 0.45) #.65
def dumpBotgal():
print "dump botgal"
#drive.withStop(100, 100, 1.0)
drive.withStop(-250, 250, 0.75)
move.cubeHolderArmParallel()
drive.withStop(-400, -400, 1.25)#was 4.0
drive.withStop(250, -250, .70)
drive.withStop(200, 200, .5)
t.sleep(10.25)
drive.withStop(-200, -200, .5)
drive.withStop(-250, 250, .65)
drive.withStop(-100,-100, 1.50)
#drive.withStop(-250, 250, .70)
drive.withStop(-200, 200, 1.125)#1.75
def driveAndReset():
print "driveAndReset"
drive.withStop( 100, 100, 3.450 ) #was 102,100
drive.withStop( -100, -100, 0.50 )#was .250
move.cubeHolderArmUp(20)
drive.withStop( -250, -250, 2.00)# was -100,-100,6.4
def endDrive():
print "endDrive"
move.cubeHolderArmMesa()
#t.sleep( 1.00 )
if c.isPrime:
drive.withStop( 102, 100, 6.500 ) #was 100,100
move.opencubeHolder( 1 ) # was .7 #dump blocks
move.cubeHolderArmMid()
drive.withStop( -200, -200, 2.600 ) #was 2.900
move.cubeHolderArmMesa()
#t.sleep( 1.00 )
drive.withStop( 100, 100, 5.300 ) #was 100,100
else:
drive.withStop( 102, 100, 6.500 ) #was 100,100
move.opencubeHolder( 1 ) #dump blocks
move.cubeHolderArmMid()
drive.withStop( -200, -200, 2.600 ) #was 2.900
move.cubeHolderArmMesa()
#t.sleep( 1.00 )
drive.withStop( 103, 100, 5.300 ) #was 100,100
def frisbeeToBotgal():
drive.withStop(200, 100, 4.00)
move.grabberArmDown()
move.frisbeeGrabberOpen()
link.create_disconnect()
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 deliverFrisbeeToNorthEndZone():
drive.withStop(200, 200, 2.00)
drive.withStop( 250, -250, 0.755 )
drive.withStop(200, 200, 1.8)
move.grabberArmDown()
move.frisbeeGrabberOpen()
def deliverFrisbeeToStartBox():
drive.withStop(300, 0, 1.2)
move.grabberArmHover()
drive.withStop(200, 200, 0.85)#100,100,2
drive.withStop( 250, -250, 0.755 )#drive.withStop(300, -300, .625)
drive.withStop(400, 400, 1.5)#100,100,9
move.cubeHolderArmMesa()
t.sleep(10.00)
drive.withStop(200, 200, 1.00)
drive.withStop( 100, -100, 2.00 )#drive.withStop(300, -300, .6)
t.sleep(1.00)
move.frisbeeGrabberOpen()
def parkInSafePlace():
print "i see nothing,"
drive.withStop(100, 100, 1.0)
drive.withStop(-50, 50, 4.00)
t.sleep(5.00)
move.cubeHolderArmParallel()
def moveToEastWall():
drive.withStop(-200, -200, 3.5)
move.cubeHolderArmParallel()
drive.withStop( -250, 250, 0.70 ) #was 0.65