def getThirdDateBin():
mpp.drive_speed(5.8, 100) #5.8
u.move_servo(c.servoPipeWheel, c.pipeStraight, 25)
mpp.rotate(95, 75) #101
mpp.drive_speed(1, 80) #2
u.move_servo(c.servoPipeWheel, 900, 20)
mpp.drive_timed(-95, -60, 2) #was -85
tim = seconds()
while analog(c.ET) < (c.onTree - 750) and seconds() - tim < 2.8:
mpp.drive_timed(-95, 60, .05) #was -85, -60
if seconds() - tim > 2.8:
print("Fourth tree timeout")
mpp.rotate(16, 80)
mpp.drive_speed(-.6, 70)
mpp.pivot_right(15, 65)
u.move_servo(c.servoPipeWheel, c.pipeBin, 25)
print "Saw Tree"
mpp.pivot_right(6, 75)
mpp.drive_speed(1.5, 80)
mpp.rotate(-10, 80) #-20
else:
mpp.pivot_right(15, 65)
u.move_servo(c.servoPipeWheel, c.pipeBin, 25)
print "Saw Tree"
mpp.pivot_right(6, 75)
mpp.drive_speed(1.5, 80)
mpp.rotate(-20, 80) # -18
mpp.drive_speed(-4.5, 80) #-6.5
mpp.pivot_left(-25, 80)
msleep(100)
mpp.pivot_right(25, 75) #was 18 degrees
mpp.new_get_poms_timed(100, 8000) #6000
mpp.pivot_right(-3, 60)
def grabFirstPomsH2H():
print "grabFirstPoms"
mpp.drive_speed(5, 85) #2.8
driveUntilTree()
msleep(100)
# mpp.drive_speed(-0.1, 80) #was -0.2
# mpp.pivot_right(6, 80) #8
mpp.drive_speed(0.2, 60) # was 0.4
mpp.pivot_right(-4, 80)
mpp.drive_speed(-0.5, 80)
mpp.pivot_right(6, 80)
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 30)
u.move_servo(c.servoDateWheel, c.wheelIn - 100, 10)
mpp.new_get_poms_timed(100, 5000)
msleep(100)
u.move_servo(c.servoDateWheel, c.wheelOut, 20)
mpp.drive_speed(2, 80)
mpp.pivot_right(4, 80)
u.move_servo(c.servoPipeWheel, c.pipeBin, 30)
mpp.drive_speed(6, 85)
driveUntilTree()
mpp.drive_speed(0.2, 60) #was 0.4
mpp.pivot_right(-4, 80)
mpp.drive_speed(-0.5, 80)
mpp.pivot_right(6, 80)
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 30)
u.move_servo(c.servoDateWheel, c.wheelIn - 50, 10)
mpp.new_get_poms_timed(100, 5000)
mpp.pivot_right(-4, 80)
def driveFirstTreesH2H():
print "driveFirstTrees"
mpp.drive_speed(-3.3, 100) #-9
msleep(100)
mpp.pivot_left(-40, 70)
msleep(100)
mpp.drive_speed(-2, 70)
mpp.drive_speed(.5, 70) # .73, 35
msleep(100)
mpp.pivot_right(8, 80) #8
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 20)
u.move_servo(c.servoDateWheel, c.wheelIn, 8)
mpp.new_get_poms_timed(100, 5000)
msleep(100)
u.move_servo(c.servoDateWheel, c.wheelOut, 20)
u.move_servo(c.servoArmBin, c.armDown, 20)
u.move_servo(c.servoPipeWheel, c.pipeBin, 20)
def startTest():
print("Running Start Test")
while analog(c.LEFT_TOPHAT) < 2000:
mpp.drive_speed(.1, 80)
msleep(750)
while analog(c.RIGHT_TOPHAT) < 1500:
mpp.drive_speed(.1, 80)
mpp.drive_speed(-9, 70)
u.move_servo(c.servoDateWheel, c.wheelOut)
print "Do the ET check!!!! (put you hand in front)"
while analog(c.ET) < 1000:
pass
u.move_servo(c.servoDateWheel, c.wheelIn)
mpp.new_get_poms_timed(50, 2000)
u.move_servo(c.servoArmBin, c.armDown, 20)
u.move_servo(c.servoArmBin, c.armUp, 20)
u.move_servo(c.servoPipeWheel, c.pipeOut, 20)
u.move_servo(c.servoPipeWheel, c.pipeBin, 20)
ao()
def driveFirstTrees():
print "driveFirstTrees"
mpp.drive_speed(-3.3, 100) #-9
msleep(100)
mpp.pivot_left(-40, 70)
msleep(100)
mpp.drive_speed(-2, 70)
mpp.drive_speed(.5, 70) # .73, 35
msleep(100)
mpp.pivot_right(6, 80) #8
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 20)
u.move_servo(c.servoDateWheel, c.wheelIn, 8)
mpp.new_get_poms_timed(100, 7000)
msleep(100)
u.move_servo(c.servoDateWheel, c.wheelOut, 20)
mpp.pivot_right(-3, 70)
mpp.drive_speed(2, 70)
mpp.pivot_right(3, 70)
mpp.pivot_right(9, 70)
def driveFinalThreeTrees():
# Goes from each of the last three trees collecting poms
print("Collect Final Three Sets of Poms")
u.move_servo(c.servoDateWheel, c.wheelOut, 25)
mpp.drive_speed(-.4, 80)
mpp.rotate(10, 80)
u.move_servo(c.servoArmBin, c.armDown, 25)
u.move_servo(c.servoPipeWheel, c.pipeBin, 25)
mpp.pivot_right(13, 85)
mpp.drive_speed(8, 85)
driveUntilTree()
# mpp.drive_speed(.3, 70) #0.5
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 30)
u.move_servo(c.servoDateWheel, c.wheelIn + 70, 10)
mpp.new_get_poms_timed(100, 7000)
msleep(100)
u.move_servo(c.servoArmBin, c.armUp, 25)
mpp.drive_speed(-2, 80)
mpp.pivot_right(-5, 80)
u.move_servo(c.servoDateWheel, c.wheelOut, 25)
u.move_servo(c.servoPipeWheel, c.pipeStraight, 25)
mpp.drive_speed(1, 80)
mpp.pivot_right(10, 80)
mpp.drive_speed(1.5, 80)
mpp.pivot_right(6, 70)
#mpp.pivot_left(-7, 70)
mpp.drive_speed(1.5, 90)
u.move_servo(c.servoPipeWheel, c.pipeBin, 25)
mpp.drive_speed(1, 85)
mpp.pivot_right(16, 80)
u.move_servo(c.servoArmBin, c.armDown, 25)
mpp.drive_speed(8, 85)
driveUntilTree()
# mpp.drive_speed(.2, 70)
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 30)
u.move_servo(c.servoDateWheel, c.wheelIn + 50, 10)
mpp.new_get_poms_timed(100, 8000)
mpp.new_get_poms_timed(100, 8000)
u.move_servo(c.servoDateWheel, c.wheelOut, 30)