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)
