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