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)
