def driveToFrisbees(): print("Driving to frisbees") x.lineFollowCondition(leftOnBlack, False) mpp.drive_speed(4, 30) #was 1 inch msleep(200) mpp.rotate(90, 30) msleep(200) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmUp) mpp.drive_condition(-40, -40, leftOnBlack, False) mpp.drive_condition(1, -20, rightOnBlack, False) mpp.rotate(-3, 20) mpp.drive_speed(-20, 80) #-12 msleep(200) mpp.drive_speed(6, 60) msleep(200) mpp.rotate(87, 30) msleep(200) mpp.drive_speed(-12, 60) #was 17 inches msleep(200) mpp.drive_speed(2.5, 60) mpp.rotate(-85, 30) msleep(500) mpp.drive_speed(-2.6, 30) mpp.drive_timed(-20, -10, .4) mpp.drive_timed(-10, -20, .4) mpp.drive_timed(-20, -10, .4) mpp.drive_timed(-10, -20, .4) mpp.drive_timed(-10, -10, .4) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmUp - 100, 3) #May need to change position bc it is too high mpp.drive_timed(-10, -10, 1) msleep(200) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmPickUp, 4) mpp.drive_speed(-2.5, 20) msleep(300)
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 driveToFrisbeesFar(): print("Driving to frisbees position 3") mpp.drive_condition(-70, -70, rightOnBlack, False) mpp.drive_condition(-20, 1, leftOnBlack, False) mpp.drive_speed(3.5, 70) mpp.rotate(90, 60) x.lineFollowConditionSlow(rightOnBlack, False) mpp.drive_speed(4, 70) #was 1 inch msleep(200) mpp.rotate(-90, 50) msleep(200) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmUp) mpp.drive_condition(-60, -60, rightOnBlack, False) mpp.drive_condition(-20, 1, leftOnBlack, False) mpp.rotate(-3, 40) mpp.drive_speed(-20, 80) #-12 mpp.drive_speed(6, 60) mpp.rotate(-87, 30) mpp.drive_speed(-12, 60) #was 17 inches mpp.drive_speed(0.4, 60) mpp.rotate(85, 30) mpp.drive_speed(-2.6, 30) mpp.drive_timed(-30, -15, .2) mpp.drive_timed(-15, -30, .2) mpp.drive_timed(-30, -15, .2) mpp.drive_timed(-15, -30, .2) mpp.drive_timed(-20, -20, .2) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmUp - 100, 3) mpp.drive_timed(-20, -20, .5) msleep(200) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmPickUp, 4) mpp.drive_speed(-2.5, 50) msleep(300)
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 self_test(): display("\nFunction: self_test\n") display("Click left button to use botguy hitter else hit right") while not right_button() and not left_button(): pass if right_button(): c.HIT_BOTGUY = False display("wont hit botguy") elif left_button(): c.HIT_BOTGUY = True display("will hit botguy") display("DONE SETTING") if u.on_black_front() or u.on_black_back(): display("Something is wrong with the tophats!") display("LTOPHAT: {}\tRTOPHAT: {}".format(u.on_black_front(), u.on_black_back())) exit(1) while not u.found_bump(): pass display("Good gyro") u.wait_for_button() enable_servos() x.drive_forever(80, 80) x.drive_condition(80, 80, u.on_black_front, False) msleep(500) x.drive_condition(80, 80, u.on_black_back, False) x.freeze_motors() u.move_servo(c.SERVO_JOINT, c.JOINT_MID) u.move_servo(c.SERVO_BIN_ARM, c.ARM_SPINNER_TEST) x.wait_for_someone_to_rotate() u.wait_for_button() x.rotate_until_stalled(20) msleep(500) x.rotate_spinner(.06, -30) msleep(500) x.set_spinner_safe() u.move_servo(c.SERVO_JOINT, c.JOINT_TUCKED) u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_OUT) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_IN) msleep(500) x.rotate(15, 60) msleep(1000) x.rotate(-15, 60) display("DONE")
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 init(): print "init" if c.isClone: print "I am Clone" else: print "I am Prime" enable_servos() u.move_servo(c.servoArm, c.armUp, 50) u.move_servo(c.servoClaw,c.clawClosed, 20) msleep(100) u.move_servo(c.servoClaw, c.clawOpen, 20) while seeHay(): pass x.drive_condition(100, 100, u.seeLine, False) x.rotate(30, 70) msleep(100) x.rotate(-30, 70) u.move_servo(c.servoArm, c.armDown, 50) u.wait4light() shut_down_in c.startTime = seconds()
def goYellowSecond(): #Maybe break this function up print('Going to yellow second position.') # Drive speed or sensor mpp.rotate(90, 30) driveToFrisbees() mpp.drive_speed(8, 50) mpp.drive_condition(60, 60, onBlack, False) mpp.drive_condition(60, 60, onBlack, True) mpp.drive_condition(60, 60, onBlack, False) mpp.drive_condition(60, 60, onBlack, True) if c.IS_ORANGE_BOT: mpp.drive_speed(1, 40) else: mpp.drive_speed(2, 45) mpp.rotate(90, 40) #line follow middle line to yellow x.lineFollowRight(3) msleep(2500) x.lineFollowConditionSlow(leftOnBlack, False) #RIGHT HERE mpp.drive_speed(7.2, 55) #8.5, 60 mpp.rotate(-90, 40) mpp.drive_condition(50, 50, onBlack, False) fasterDropOffCrates() mpp.drive_speed(3.5, 60) #-1 mpp.drive_speed(-.5, 60) mpp.drive_speed(1, 60) mpp.drive_speed(-5, 60) msleep(200) #drop off frisbee u.move_servo(c.servoArm, c.armUp, 25) u.move_servo(c.servoClaw, c.clawClosed, 25) mpp.rotate(-150, 50) mpp.drive_timed(60, 60, 2.5) msleep(10000) mpp.drive_timed(-60, -60, 2.5) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmHorizontal, 4) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmDown) u.DEBUG()
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 goYellowThird(): print('Going to yellow third position.') mpp.rotate(-70, 50) #Change these to use the corner black line #x.lineFollowCondition(leftOnBlack, False) if c.IS_ORANGE_BOT: x.lineFollowConditionSlow(rightOnBlack, False) mpp.drive_speed(-4, 40) else: x.lineFollowConditionSlow(rightOnBlack, False) mpp.drive_speed(-4, 40) #x.lineFollowRight(6.3) mpp.rotate(-90, 40) mpp.drive_condition(-40, -40, rightOnBlack, False) mpp.drive_condition(-20, 1, leftOnBlack, False) mpp.rotate(-3, 30) #RIGHT HERE mpp.drive_condition(60, 60, onBlack, False) msleep(9000) dropOffCrates() mpp.drive_speed(-1, 50) mpp.drive_speed(3, 50) mpp.drive_speed(-2, 50) mpp.drive_speed(3.5, 65) mpp.drive_speed(-3.5, 65)
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 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 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 goYellowFirst(): print('Going to yellow first position.') mpp.rotate(90, 30) # Change these to use the corner black line # x.lineFollowCondition(leftOnBlack, False) if c.IS_ORANGE_BOT: x.lineFollowLeft(5.1) else: x.lineFollowLeft(5) #4.3 #Do we need to change to a stop at corner black tape mpp.rotate(90, 30) mpp.drive_speed(9, 50) mpp.drive_condition(-10, -10, onBlack, True) dropOffCrates() mpp.drive_speed(2, 50) mpp.drive_speed(-3, 50) mpp.drive_speed(4, 50) #Different pattern if c.IS_ORANGE_BOT: mpp.drive_speed(-6, 50) else: mpp.drive_speed(-7, 50) mpp.arc_radius(90, 4.3, -60) mpp.drive_speed(-5, 60)
def dropOffFrisbee(): u.move_servo(c.servoArm, c.armUp) u.move_servo(c.servoClaw, c.clawClosed) mpp.drive_speed(8, 50) mpp.drive_condition(50, 50, onBlack, False) mpp.drive_condition(50, 50, onBlack, True) mpp.drive_condition(50, 50, onBlack, False) mpp.drive_condition(50, 50, onBlack, True) mpp.rotate(90, 30) x.lineFollowLeft(3.7) mpp.rotate(90, 30) #RIGHT HERE if c.IS_ORANGE_BOT: mpp.drive_speed(-6.5, 30) #was 8 inches else: mpp.drive_speed(-5.5, 28) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmHorizontal, 4) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmDown)
def selfTest(): enable_servos() print("Hola amigos") u.move_servo(c.servoArm, c.armMid) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmStartPosition) mpp.drive_condition(60, 60, leftOnBlack, False) msleep(500) mpp.drive_condition(-60, -60, onBlack, False) msleep(500) mpp.drive_condition(60, 60, backOnBlack, False) msleep(500) mpp.drive_condition(-60, -60, rightOnBlack, False) mpp.rotate(-20, 30) mpp.rotate(20, 30) msleep(750) u.move_servo(c.servoClaw, c.clawOpen) u.move_servo(c.servoClaw, c.clawClosed) u.move_servo(c.servoClaw, c.clawFullyOpen) u.move_servo(c.servoArm, c.armStartBoxPosition) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmDown) u.move_servo(c.servoFrisbeeArm, c.frisbeeArmStartPosition) mpp.drive_timed(-70, -70, 2)
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)