def dropPomsInFurrow():
x.drive_speed(9.5, 100)
if c.isClone:
x.pivot_right(11, 50)
else:
x.pivot_right(13, 50)
x.drive_speed(7, 100)
if c.isClone:
u.move_servo(c.servoArm, c.armDownMid, 40)
else:
u.move_servo(c.servoArm, c.armDown, 25)
#x.drive_speed(4, 100)
msleep(500)
u.move_servo(c.servoClaw, c.clawOpen, 20)
msleep(500)
u.move_servo(c.servoArm, c.armMid, 20)
msleep(100)
u.move_servo(c.servoClaw, c.clawClosed, 250)
msleep(200)
u.move_servo(c.servoArm, c.armDown, 35)
msleep(100)
u.move_servo(c.servoArm, c.armMid, 35)
msleep(100)
u.move_servo(c.servoArm, c.armDown, 35)
msleep(100)
u.move_servo(c.servoArm, c.armUp, 35)
msleep (100)
u.move_servo(c.servoClaw, c.clawOpen, 250)
def go_and_score_the_bin():
display("\nFunction: go_and_score_the_bin\n")
u.move_servo(c.SERVO_JOINT, c.JOINT_DELIVER, 4)
msleep(500)
u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_OUT, 100)
# x.linefollow_distance(28, 50, 70)
x.linefollow_distance(21, 50, 70, 5)
x.pivot_right(-32.5, 50)
# x.drive_speed(-2, 50)
# x.rotate(-10, 50)
# x.drive_speed(2.5, 50)
# x.pivot_right(40, 50)
# x.drive_speed(2.5, 50)
# u.wait_for_button()
if not c.IS_CLONE:
x.drive_speed(1, 50)
disable_servo(c.SERVO_JOINT)
msleep(500)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_MAX)
msleep(500)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_HIT, 20)
msleep(300)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_MAX, 20)
x.drive_speed(1, 50)
x.pivot_right(30, 50)
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)
def grabFirstPoms():
print "grabFirstPoms"
mpp.drive_speed(2.8, 85)
driveUntilTree()
msleep(100)
#mpp.drive_speed(-0.2, 80)
mpp.pivot_right(6, 80) #8
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.6, 60)
mpp.pivot_right(-4, 80)
mpp.drive_speed(-0.7, 80) #was -0.5
mpp.pivot_right(6, 80) #was 6
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 get_bin():
display("\nFunction: get_bin\n")
u.move_servo(c.SERVO_JOINT, c.JOINT_TUCKED, 100)
if c.IS_CLONE:
x.rotate(-86, 50)
else:
x.rotate(-86, 50)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_APPROACH)
u.move_servo(c.SERVO_JOINT, c.JOINT_SWING)
msleep(250)
if c.IS_CLONE:
x.drive_speed(12, 70)
else:
x.drive_speed(10, 70)
u.move_servo(c.SERVO_JOINT, c.JOINT_SWING)
u.move_bin(c.ARM_SWING, 5)
u.move_servo(c.SERVO_JOINT, c.JOINT_PARALLEL, 5)
u.move_bin(c.ARM_APPROACH, 5)
u.move_servo(c.SERVO_JOINT, c.JOINT_ROTATE, 5)
# x.drive_speed(-20, 100)
x.drive_speed(-16, 100)
x.drive_speed(-4, 50)
if c.HIT_BOTGUY:
u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_OUT, 100)
x.pivot_right(30, 75)
x.pivot_right(-30, 75)
u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_IN, 100)
def exitMiddle():
x.drive_speed(-2,100)
x.pivot_right(90, 100)
x.drive_speed(-6.5, 50)
if c.isClone:
x.drive_timed(100,100,4.5)
else:
x.drive_timed(100, 100, 4)
x.rotate(-75, 75)
x.drive_condition(100, 100, u.seeLine, False)
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 go_to_ramp():
display("\nFunction: go_to_ramp\n")
u.move_servo(c.SERVO_JOINT, c.JOINT_RAMP_ON)
u.move_servo(c.SERVO_JOINT, c.JOINT_ARM_TILT)
if c.IS_CLONE:
x.rotate(-5, 50)
else:
x.rotate(-5, 50)
x.drive_forever(-50, -50)
u.move_bin(c.ARM_TILT, 5)
x.drive_speed(-10, 100)
x.rotate(5, 65)
u.move_servo(c.SERVO_JOINT, c.JOINT_HOLD, 5)
u.move_bin(c.ARM_TUCKED, 5)
msleep(100)
x.drive_speed(-7, 100)
x.drive_speed(-6, 75)
x.drive_speed(2, 75)
x.pivot_right(-90, 60)
u.move_servo(c.SERVO_JOINT, c.JOINT_MID)
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)
def driveToCrates():
print("Driving to crates")
u.move_servo(c.servoArm, c.armBlockLevel)
u.move_servo(c.servoClaw, c.clawOpen)
if c.IS_ORANGE_BOT:
mpp.drive_speed(-1, 50)
else:
mpp.drive_speed(1.5, 60)
mpp.pivot_right(84, 50)
mpp.drive_till_black(60, 60)
#Just added this to prevent turn off line in line follow
mpp.drive_speed(2, 50)
#msleep(1000)
x.lineFollowConditionSlow(backOnBlack, False)
u.move_servo(c.servoClaw, c.clawOpen + 100)
mpp.drive_timed(50, 30, 0.8)
u.move_servo(c.servoClaw, c.clawClosed)
msleep(600)
u.move_servo(c.servoArm, c.armMid, 4)
msleep(500)
mpp.drive_condition(-90, -100, leftOnBlack, False)
#mpp.drive_speed(-7, 40)
u.move_servo(c.servoArm, c.armHighMid, 4)
def driveToCenter():
print("Driving to center")
mpp.drive_speed(4, 50)
mpp.rotate(50, 40)
mpp.drive_speed(24.5, 50)
mpp.pivot_right(-15, 30)
mpp.pivot_right(57, 30)
mpp.drive_speed(11, 50)
mpp.pivot_right(-90, 30)
def getToMiddle():
x.drive_speed(-4, 100)
x.pivot_right(-90, 100)
x.drive_speed(-5.5, 50)
if c.isClone:
x.drive_speed(31,100)
else:
x.drive_speed(28, 100)
x.rotate(90, 50)
x.drive_condition(100,100,u.seeLine,False)
x.drive_speed(-1,100)
x.rotate(-40,100)
x.drive_condition(100, 100, u.seeLine, False)
# x.pivot_left_condition(60, u.seeLine)
x._drive(60, 0)
while u.seeLine():
pass
if c.isClone:
lineFollowRightTimed(8)
else:
lineFollowRightTimed(7)
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 goToBarn():
x.drive_speed(3, -100)
u.move_servo(c.servoArm, c.armDown, 15)
x.drive_speed(30, -100)
u.DEBUG()
u.move_servo(c.servoArm, c.armUp, 30)
x.pivot_right(-85, 50)
x.drive_speed(6, 100)
x.pivot_right(90, 50)
x.drive_speed(-30,100)
u.DEBUG()
x.drive_condition(100, 100, u.seeLine, False)
x.drive_condition(-50, -50, u.seeLine)
u.move_servo(c.servoArm, c.armFurrow, 15)
msleep(100)
u.DEBUG()
x.drive_speed(3, -100)
u.move_servo(c.servoArm, c.armDown, 15)
x.drive_speed(5, -100)
u.move_servo(c.servoArm, c.armUp, 30)
x.pivot_right(-85, 50)
x.drive_speed(-30,100)
def getSecondDateBin():
mpp.drive_speed(4, 70)
u.move_servo(c.servoArmBin, c.armDown, 30)
mpp.drive_speed(2.5, 70)
u.move_servo(c.servoPipeWheel, c.pipeBin, 20)
mpp.pivot_right(6, 70)
def getThirdDateBinH2H():
mpp.drive_speed(5.9, 100) #5.8
u.move_servo(c.servoPipeWheel, c.pipeStraight, 25)
mpp.rotate(95, 75) #93
mpp.drive_speed(10, 80) #was 8
u.move_servo(c.servoPipeWheel, 900, 20)
mpp.drive_timed(-98, -60, 2) #was -95, -60
tim = seconds()
while analog(c.ET) < (c.onTree - 750) and seconds() - tim < 2.8:
mpp.drive_timed(-98, -60, .05) #was -95, -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(-5.5, 80) #-4.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)
u.move_servo(c.servoDateWheel, c.wheelOut, 25)
mpp.drive_speed(-0.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)
def dropSecondPoms():
x.rotate(30,75)
x.drive_speed(15,100)
# x.rotate(160,75)
x.pivot_right(140, 75)
x.drive_speed(-10,75)
x.drive_speed(3.9,75)
x.rotate(90,75)
x.drive_speed(-10,75)
u.move_servo(c.servoArm, c.armDown, 10)
#x.drive_condition(95,100, u.seeLine,False)
if c.isClone:
x._drive(90,100)
msleep(2100)
else:
x._drive(100,100)
msleep(1500)
u.move_servo(c.servoClaw, c.clawClosed,20)
x._drive(0, 0)
u.move_servo(c.servoArm, c.armUp, 100)
x.rotate(-90,60)
x.drive_speed(-5,100)
x.drive_speed(60, 88)
x.rotate(90, 60)
x.drive_speed(-27, 100)
msleep(3000)
x.drive_speed(29, 100)
x.rotate(95, 60)
if c.isClone:
x.drive_speed(-25,100)
x.drive_speed(14.75, 100)
x.rotate(-85, 75)
x.drive_speed(16.5,100)
x.drive_speed(-1, 100)
else:
x.drive_speed(-30,100)
x.drive_speed(14.75, 100)
x.rotate(-90, 75)
x.drive_condition(100, 100, u.seeLine, False)
x.drive_condition(25, 25, u.seeLine)
u.move_servo(c.servoArm, c.armDownMid,25)
u.move_servo(c.servoClaw, c.clawOpen,20)
u.move_servo(c.servoArm,c.armMid,20)
msleep(100)
u.move_servo(c.servoClaw, c.clawClosed, 200)
msleep(100)
u.move_servo(c.servoArm, c.armDown, 35)
msleep(100)
u.move_servo(c.servoArm, c.armMid, 35)
msleep(100)
u.move_servo(c.servoArm, c.armDown, 35)
msleep(100)
u.move_servo(c.servoArm, c.armMid, 35)
msleep(100)
u.move_servo(c.servoClaw, c.clawOpen, 200)
msleep(500)
print 'Program stop for DEBUG\nSeconds: ', seconds() - c.startTime
x.drive_condition(-50, -50, u.seeLine, False)
x.drive_condition(-50, -50, u.seeLine)
u.move_servo(c.servoArm, c.armFurrow, 15)
msleep(100)
def driveFinalThreeTrees():
# Goes from each of the last three trees collecting poms
print("Collect Final Three Sets of Poms")
if c.isYellow:
mpp.drive_speed(3, 40)
mpp.rotate(-28, 50)
msleep(1000)
#mpp.drive_speed(-7, 50)
mpp.drive_timed(-75,-100,1.5)
u.move_servo(c.servoArmBin, c.armDown)
u.waitForButton()
mpp.get_poms_timed(50, 7000)
msleep(1000)
driveUntilTree()
mpp.get_poms_timed(50, 7000)
msleep(1000)
driveUntilTree()
mpp.get_poms_timed(50, 7000)
msleep(1000)
if c.isGreen:
mpp.drive_speed(-3, 40) #was -9.5
u.waitForButton()
mpp.rotate(-14, 50)
mpp.drive_speed(-8, 40)
u.waitForButton()
u.move_servo(c.servoArmBin, c.armDown)
mpp.pivot_right(17, 25)
mpp.drive_speed(.2, 50)
u.waitForButton()
mpp.get_poms_timed(50, 9300)
msleep(1000)
u.DEBUG()
driveUntilTree()
mpp.drive_speed(.2, 50)
mpp.get_poms_timed(50, 9300)
msleep(1000)
######ET isn't low enough right now
#driveUntilTree()
#mpp.drive_speed(.2, 50)
#mpp.get_poms_timed(50, 8300)
#msleep(1000)
mpp.pivot_right(-8, 25) # -8
mpp.drive_speed(4, 50)
mpp.pivot_right(8, 25)
mpp.drive_speed(9.5, 50)
mpp.pivot_right(4, 25)
mpp.get_poms_timed(50, 9300)
if c.isBlue:
mpp.drive_speed(3, 40)
mpp.rotate(-28, 50)
msleep(1000)
#mpp.drive_speed(-7, 50)
mpp.drive_timed(-75,-100,1.5)
u.move_servo(c.servoArmBin, c.armDown)
u.waitForButton()
mpp.get_poms_timed(50, 7000)
msleep(1000)
driveUntilTree()
mpp.get_poms_timed(50, 7000)
msleep(1000)
driveUntilTree()
mpp.get_poms_timed(50, 7000)
msleep(1000)
def driveUntilTree():
#Helper function for driveFirstThreeTrees()
#Drives from one date tree to the next
print("Looking for Trees")
if (c.isBlue):
mpp.pivot_right(-8, 25) # -8
mpp.drive_speed(4, 50)
mpp.pivot_right(8, 25)
while analog(c.ET) < c.onTree:
mpp.drive_timed(50, 50, 0.01)
print(analog(c.ET))
print("Saw Tree")
mpp.pivot_right(2, 25) # 10
elif c.isGreen:
mpp.pivot_right(-8, 25)#-8
mpp.drive_speed(4, 50)
mpp.pivot_right(8, 25)
while analog(c.ET) < c.onTree:
mpp.drive_timed(50, 50, 0.01)
print(analog(c.ET))
print("Saw Tree")
mpp.pivot_right(2, 25)#10
mpp.drive_speed(1.4, 50)#1.4
# mpp.drive_timed(5, 40, 1.5)
#mpp.drive_speed(2, 50)
#u.waitForButton()
elif c.isYellow:
mpp.pivot_right(-8, 25)
mpp.drive_speed(6, 50)
mpp.pivot_right(4, 25)
while analog(c.ET) < c.onTree:
mpp.drive_timed(50, 50, 0.01)
print(analog(c.ET))
print("Saw Tree")
mpp.pivot_right(9, 25) #was 5
mpp.drive_speed(1.2, 50) #was 2.5
