def driveFirstTreesExperiment(): if c.isGreen: mpp.drive_speed(-7, 50) mpp.drive_speed(0.5, 20) # .73 msleep(250) #motor_power(c.RMOTOR, 10) mpp.get_poms_timed(70, 6000) msleep(1000) driveUntilTree() u.move_servo(c.servoArmBin, c.armDown) u.waitForButton() mpp.get_poms_timed(70, 6000) msleep(1000) driveUntilTree() mpp.get_poms_timed(70, 6000) msleep(1000) elif c.isBlue: mpp.drive_speed(-7, 50) mpp.drive_speed(0.5, 20) # .73 msleep(250) # motor_power(c.RMOTOR, 10) mpp.get_poms_timed(50, 9000) msleep(1000) u.move_servo(c.servoPipeWheel,c.pipeStraight) u.waitForButton() driveUntilTree() mpp.drive_speed(2.85,50) u.move_servo(c.servoPipeWheel,c.pipeBin) u.move_servo(c.servoArmBin, c.armDown) mpp.get_poms_timed(50, 9000) msleep(1000) driveUntilTree() mpp.get_poms_timed(60, 6000) msleep(1000)
def driveToNextTrees(): #Uses an amazingly smooth line follow to go from one set of trees to the other print("Driving to Next Set of Trees") u.move_servo(c.servoArmBin, c.armUp, 25) mpp.drive_speed(-4.7, 60) mpp.rotate(-90, 50) mpp.drive_speed(-7, 80) msleep(500) mpp.drive_speed(17, 80)#14 msleep(500) mpp.rotate(89, 50) msleep(1000) if c.isGreen: u.smoothLineFollowLeft(7.2, 80) elif c.isBlue: u.smoothLineFollowLeft(7.5, 80) elif c.isYellow: u.smoothLineFollowLeft(5, 80) #mpp.drive_speed(44, 90) msleep(500) if c.isGreen: mpp.rotate(-95, 50) else: mpp.rotate(-90, 50) mpp.drive_speed(-20, 80) msleep(500) mpp.drive_speed(5.5, 80) if c.isGreen: mpp.rotate(94.5, 50) elif c.isBlue: mpp.rotate(93, 50) elif c.isYellow: mpp.rotate(93, 50) msleep(500)
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 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 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 alt_init(): u.move_servo(c.SERVO_JOINT, c.JOINT_MID, 2047) u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED, 2047) enable_servos() u.wait_for_button() # u.move_bin(c.ARM_SWING) # u.move_bin(c.ARM_ALL_UP) x.drive_speed(70, 100)
def driveOutStartBox(): #drives out of start box to pom if c.IS_ORANGE_BOT: # mpp.drive_timed(85, 105, 2.7) mpp.drive_speed(16, 90) else: #IS_BLUE_BOT # mpp.drive_timed(100, 79, 2.6) # original mpp.drive_speed(16, 90) # test
def startTest(): print("Running Start Test") mpp.drive_speed(3, 30) mpp.drive_speed(-3, 30) u.move_servo(c.servoArmBin, c.armUp) u.move_servo(c.servoArmBin, c.armDown) u.move_servo(c.servoPipeWheel, c.pipeOut) u.move_servo(c.servoPipeWheel, c.pipeBin) mpp.drive_date_motor(50, 1000)
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 grabCowAndGo(): u.move_servo(c.servoCowClaw, c.cowClawOpen, 10) msleep(500) #u.move_servo(c.servoCow, c.cowDown, 10) msleep(1000) u.move_servo(c.servoCowClaw, c.cowClawClose, 10) u.waitForButton() #u.move_servo(c.servoCow, c.cowUp,10) msleep(300) x.drive_speed(34, 100)
def driveUntilTree(): #Helper function for driveFirstThreeTrees() #Drives from one date tree to the next print("Looking for Trees") sec = seconds() + 5 while analog(c.ET) < c.onTree and seconds() < sec: mpp.drive_speed(.05, 90) print(analog(c.ET)) print("Saw Tree") u.move_servo(c.servoArmBin, c.armDown, 20)
def test(): print"test" if c.isClone: print "I am Clone" else: print "I am Prime" enable_servos u.move_servo(c.servoArm, c.armUp) msleep(200) x.drive_speed(50,100) u.DEBUG()
def new_get_poms_timed(speed, time): for x in range(3): get_poms_wiggle(speed, time / 3) motor(c.DATEMOTOR1, 0) motor(c.DATEMOTOR2, 0) u.move_servo(c.servoDateWheel, c.wheelIn + 300, 20) u.move_servo(c.servoDateWheel, c.wheelIn, 20) pivot_right(1, 60) pivot_left(.5, 60) mpp.drive_speed(-.25, 60) motor(c.LMOTOR, 0) motor(c.RMOTOR, 0)
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 upRamp(): x.rotate(25, 50) x.drive_speed(36, 100) x.drive_speed(6, -50) x.rotate(89, 50) x.drive_speed(30, 100) x.drive_speed(4, -50) x.rotate(79, 50) u.move_servo(c.servoArm, c.armBotguy,10) msleep(300) x.drive_speed(70, 75) u.move_servo(c.servoCow, c.cowDown, 10) msleep(300) x.rotate(-45, 50) print "Seconds elapsed: " + seconds() - c.startTime
def dropOffFarFrisbee(): u.move_servo(c.servoArm, c.armUp) u.move_servo(c.servoClaw, c.clawClosed) msleep(300) mpp.drive_speed(6, 70) msleep(200) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmUp) mpp.rotate(-90, 30) msleep(200) mpp.drive_speed(-12, 60) # was 17 inches msleep(200) mpp.drive_speed(1.5, 60) mpp.rotate(90, 30) msleep(500) mpp.drive_speed(-4.3, 40) msleep(200) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmPickUp, 5) mpp.drive_speed(-2.5, 20)
def goToCow(): msleep(300) #u.move_servo(c.servoCow, c.cowUp, 10) msleep(300) x.drive_speed(7, -50) x.rotate(-90, 20) x.drive_speed(14, 50) x.rotate(185, 20) x.drive_speed(2, 50) u.DEBUG() x.drive_speed(9, -50)
def dropOffFrisbeeFar(): u.move_servo(c.servoArm, c.armUp, 20) u.move_servo(c.servoClaw, c.clawClosed, 20) mpp.drive_speed(8, 70) mpp.drive_condition(70, 70, onBlack, False) mpp.drive_condition(70, 70, onBlack, True) mpp.drive_condition(70, 70, onBlack, False) mpp.drive_condition(70, 70, onBlack, True) if c.IS_ORANGE_BOT: mpp.drive_speed(1.5, 70) else: pass mpp.rotate(-75, 50) x.lineFollowLeft(4.8) mpp.rotate(-90, 50) mpp.drive_condition(-50, -50, onBlack, False) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmHorizontal, 4) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmDown) u.DEBUG()
def leave_startbox(): display("\nFunction: leave_startbox\n") u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED) x.drive_condition(80, 80, u.on_black_front, False) x.drive_speed(-4, 60) if c.IS_CLONE: x.rotate(-92, 70) else: x.rotate(-96, 70) x.drive_speed(-34, 100) x.drive_condition(80, 80, u.on_black_front, False) x.drive_speed(1, 80) x.rotate(92, 60) x.drive_speed(-7, 85)
def go_up_ramp(): display("\nFunction: go_up_ramp\n") u.move_bin(c.ARM_SWING) x.drive_speed(12, 100) start_time = seconds() x.drive_speed(5, 100) while gyro_y() < 100 or seconds() < start_time + 2: if u.on_black_front(): x.drive_forever(50, 100) else: x.drive_forever(100, 50) msleep(10) x.drive_speed(8, 100) u.move_servo(c.SERVO_JOINT, c.JOINT_GROUND) # u.wait_for_button() print("1") x.pivot_left_condition(30, u.on_black_front, False) # u.wait_for_button() print("2") # if u.on_black_back(): x.pivot_right_condition(30, u.on_black_back) # x.pivot_right(35, 30) # u.wait_for_button() print("3") x.pivot_right_condition(30, u.on_black_back, False) # u.wait_for_button() print("4") x.pivot_left_condition(30, u.on_black_front, False) # u.wait_for_button() print("5") u.move_bin(c.ARM_ALL_UP) msleep(500)
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 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
def fasterDropOffCrates(): mpp.rotate(-4, 50) u.move_servo(c.servoArm, c.armBlockLevel, 15) u.move_servo(c.servoClaw, c.clawOpen, 15) mpp.drive_speed(1.5, 70) u.move_servo(c.servoArm, c.armDestack, 15) mpp.rotate(1, 40) u.move_servo(c.servoClaw, c.clawClosed, 15) u.move_servo(c.servoArm, c.armUp, 15) mpp.rotate(-5, 40) mpp.drive_speed(-3, 60) mpp.rotate(-82, 40) #-86 mpp.drive_speed(12, 50) mpp.rotate(90, 40) mpp.drive_speed(.5, 50) u.move_servo(c.servoArm, c.armBlockLevel) u.move_servo(c.servoClaw, c.clawFullyOpen)
def dropOffCrates(): u.move_servo(c.servoArm, c.armBlockLevel) u.move_servo(c.servoClaw, c.clawOpen) mpp.drive_speed(1.5, 50) # .5 u.move_servo(c.servoArm, c.armDestack) mpp.rotate(1, 10) u.move_servo(c.servoClaw, c.clawClosed) u.move_servo(c.servoArm, c.armUp) mpp.rotate(-5, 10) mpp.drive_speed(-6, 30) # -5 msleep(500) mpp.rotate(-88, 30) # 90 mpp.drive_speed(10.5, 50) mpp.rotate(90, 30) # 96 msleep(200) #Initial value? mpp.drive_speed(3.5, 50) u.move_servo(c.servoArm, c.armBlockLevel) u.move_servo(c.servoClaw, c.clawFullyOpen)
def test_ramp(): x.drive_speed(100, 100) u.DEBUG() enable_servos() u.move_servo(c.DEPLOYABLE_WHEELS, c.WHEELS_DEPLOYED) u.move_bin(c.ARM_SWING) x.drive_speed(12, 100) start_time = seconds() x.drive_speed(5, 100) while gyro_y() < 100 or seconds() < start_time + 2: if u.on_black_front(): x.drive_forever(70, 100) else: x.drive_forever(100, 70) msleep(10) x.drive_speed(4, 100) x.pivot_left_condition(30, u.on_black_front, False)
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_up_ramp2(): display("\nFunction: go_up_ramp\n") u.move_bin(c.ARM_SWING) x.drive_speed(12, 100) start_time = seconds() x.drive_speed(5, 100) while gyro_y() < 100 or seconds() < start_time + 2: if u.on_black_front(): x.drive_forever(70, 100) else: x.drive_forever(100, 70) msleep(10) x.freeze_motors() u.move_servo(c.SERVO_JOINT, c.JOINT_GROUND) if c.IS_CLONE: x.drive_speed(5, 100) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_ET) x.pivot_right_condition(-30, u.lost_ramp, False) x.pivot_left_condition(-30, u.on_black_back, False) x.linefollow_distance(9) if not c.IS_CLONE: x.drive_speed(8, 100) # u.wait_for_button() print("1") x.pivot_left_condition(30, u.on_black_front, False) # u.wait_for_button() print("2") # if u.on_black_back(): x.pivot_right_condition(30, u.on_black_back) # x.pivot_right(35, 30) # u.wait_for_button() print("3") x.pivot_right_condition(30, u.on_black_back, False) # u.wait_for_button() print("4") x.pivot_left_condition(30, u.on_black_front, False) # u.wait_for_button() print("5") u.move_bin(c.ARM_ALL_UP) msleep(500)
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 getBotGuy(): x.drive_speed(28.5, 100) x.rotate(90, 50) u.move_servo(c.servoArm, c.armBotguy, 10) u.move_servo(c.outrigger, c.outriggerMid, 10) x.drive_speed(16, 100) u.move_servo(c.servoArm, c.armDown, 10) msleep(300) u.move_servo(c.servoClaw, c.clawClose, 10) msleep(500) u.move_servo(c.servoArm, c.armUp, 10) x.drive_speed(7, -20) u.move_servo(c.outrigger, c.outriggerIn, 10) msleep(300) u.DEBUG() u.move_servo(c.servoArm, c.armUpBotguy, 10)