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 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 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 go_to_spinner():
display("\nFunction: go_to_spinner\n")
u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED, 5)
if c.IS_CLONE:
x.drive_speed(8, 100)
else:
x.drive_speed(11, 100)
if c.IS_CLONE:
x.pivot_left(-90, 70)
else:
x.pivot_left(-88, 70)
x.drive_speed(22, -100, True)
x.pivot_left(-32, 50)
x.drive_speed(-11, 80)
x.pivot_right(-32, 50)
x.drive_speed(-3, 70)
x.drive_condition(50, 50, u.on_black_front, False)
if c.IS_CLONE:
x.rotate(90, 35)
else:
x.rotate(98, 35)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED)
u.move_servo(c.SERVO_JOINT, c.JOINT_PARALLEL)
x.drive_condition(80, 80, u.on_black_front, False)
x.drive_condition(50, 50, u.on_black_front, True)
x.rotate_spinner(.25, 80)
x.drive_speed(5, 60)
u.move_servo(c.SERVO_JOINT, c.JOINT_GROUND)
x.rotate_spinner(4, -70)
x.rotate_to_safe(50)