def driveOutStartBox():
#Starts from the start box, drives to first date tree and positions itself to collect
print("Drive Out of Start Box")
if (c.isBlue):
#drives out of start box to pom
mpp.drive_speed(4, 80)
mpp.rotate(-89, 50)
u.DEBUG()
u.move_servo(c.servoArmBin, c.armUp)
mpp.drive_speed(-27.5, 80) #27
mpp.rotate(90, 50)
mpp.drive_speed(-6, 40) #5
mpp.drive_speed(3, 40)
mpp.rotate(-30, 50) # was -30
msleep(1000)
elif c.isGreen:
mpp.drive_speed(3.5, 80) # 9.4
mpp.rotate(-92, 50)#-90
u.move_servo(c.servoArmBin, c.armUp)
mpp.drive_speed(-26.9, 80)
mpp.rotate(90, 50)
mpp.drive_speed(-5, 40)
mpp.drive_speed(3, 40)
mpp.rotate(-28, 50) #was -30
msleep(1000)
elif c.isYellow:
mpp.drive_speed(3.5, 80)
mpp.rotate(-80, 50)
u.move_servo(c.servoArmBin, c.armUp)
mpp.drive_speed(-27, 80) #was -26
mpp.rotate(85, 50) #was 78
mpp.drive_speed(-6, 40)
mpp.drive_speed(3, 40)
mpp.rotate(-30, 50)
msleep(1000)
def main():
print("Running")
act.init()
act.driveOutStartBox()
act.driveFirstTreesExperiment()
#act.driveFirstThreeTrees()
act.driveToNextTrees2()
#act.driveToNextTrees()
act.driveFinalThreeTrees()
u.DEBUG()
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 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 main():
print("Running the code")
# mpp.drive_speed(36,100)
# msleep(500)
# u.DEBUG()
act.init()
act.driveOutStartBox()
act.seeBlocks()
act.driveToSecondBlock()
act.seeBlocksTwo()
act.driveToCrates()
act.driveToYellow()
u.DEBUG()
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 init():
# starting positions
# START WITH CLAW UP/OPEN!!!!!!!!!
if c.IS_ORANGE_BOT:
print("I AM ORANGE BOT!")
elif c.IS_BLUE_BOT:
print("I AM BLUE BOT!")
else:
print("I AM YELLOW BOT! !!!!!!!!!!")
u.DEBUG() # do not remove
selfTest()
p.cameraInit()
msleep(100)
u.wait_4_light()
shut_down_in(119.5)
c.startTime = seconds()
def main():
print("Running the code")
#Code moves ring to top rung and then pushes the tram to the middle (usually)
act.init()
act.turnToRing()
act.liftRing()
act.raiseRing2()
act.dropRing()
act.turnToTram()
act.slideTram()
act.approachCenter()
act.approachBotguy()
act.deliverBotguy()
u.DEBUG()
act.getFrisbee()
create_disconnect()
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)
def grab_poms():
#g.drive_distance(100, 2)
if (c.IS_CLONE):
print "Don't go any further, test all servo positions so that you don't break anything"
u.DEBUG()
d.line_follow_until_switch()
g.drive_distance(-80, 3) #4
u.move_servo(c.FRONT_ARM, c.FA_DOWN1)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN_BIN, 7)
g.drive_distance(60, .6)
u.move_servo(c.FRONT_ARM, c.FA_DOWN2, 7)
g.drive_distance(60, 1)
u.move_servo(c.FRONT_ARM, c.FA_DOWN3, 7)
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED_BIN, 5)
u.move_servo(c.FRONT_ARM, c.FA_DOWN2, 7)
g.drive_distance(-60, .9)
u.move_servo(c.FRONT_ARM, c.FA_DOWN1, 7)
g.drive_distance(-60, .6)
u.move_servo(c.FRONT_ARM, c.FA_MID, 7)
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 main():
a.init()
a.lower_ramp()
a.move_coupler_to_blocks()
a.back_up_around_blocks()
a.back_to_up_ramp_position()
a.go_up_ramp()
a.grab_poms()
a.deliver_poms()
a.smoosh_poms()
a.return_to_poms()
a.second_grab_poms()
a.deliver_poms()
a.smoosh_poms()
u.move_servo(c.FRONT_ARM, c.FA_UP)
a.get_back_down_from_ramp()
a.get_back_to_coupler()
a.return_to_start()
u.DEBUG()
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 main():
#########################HEY READ THIS IF UR NOT AJ OR KAT :)###############################
#So we have both head 2 head and seeding fairly consistent for blue bot
#We've gotten 4800 max for seeding and 2100 max for head 2 head!!!
#The next step is to work on the clone (seeding and head 2 head)
#We've started working on seeding for clone (Green Bot) but we've only finished the first tree
#So work on that ^^^^^^^^^^^^^^^
#We also need to work on Head 2 Head for clone
#Good Luck!
print("Running code")
#Calibration drive
#mpp.calibrate_drive()
#u.DEBUG()
print "Code Sending"
act.init()
if u.wait_for_selection():
#Seeding
u.wait_4_light()
c.startTime = seconds()
shut_down_in(119.5)
print("Running Seeding")
act.driveOutStartBox()
act.driveFirstTrees()
act.getSecondDateBin()
act.grabFirstPoms()
act.driveToNextTrees()
act.getThirdDateBin()
act.driveFinalThreeTrees()
else:
#Head 2 Head
u.wait_4_light()
c.startTime = seconds()
shut_down_in(119.5)
print("Running Head 2 Head")
act.driveOutStartBoxH2H()
act.driveFirstTreesH2H()
act.grabFirstPomsH2H()
act.driveToNextTrees()
act.getThirdDateBinH2H()
act.grabFirstPomsH2H()
u.DEBUG()
def second_grab_poms():
if (c.IS_CLONE):
print "Don't go any further, test all servo positions so that you don't break anything"
u.DEBUG()
d.line_follow_until_switch()
g.drive_distance(-80, 3)
u.move_servo(c.FRONT_ARM, c.FA_DOWN1)
u.move_servo(c.FRONT_CLAW, c.FC_LESS_OPEN_BIN, 7)
g.drive_distance(60, .6)
u.move_servo(c.FRONT_ARM, c.FA_DOWN2, 7)
g.drive_distance(60, 2.0) # 1 eeeee
u.move_servo(c.FRONT_ARM, c.FA_DOWN3, 7)
u.move_servo(c.LEFT_ARM, c.LA_BACK)
g.drive_distance(60, 1.5)
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED_BIN, 5)
g.drive_distance(-60, 1.4)
u.move_servo(c.FRONT_ARM, c.FA_DOWN2, 7)
g.drive_distance(-60, 1.9) # 0.9 eeeee
u.move_servo(c.FRONT_ARM, c.FA_DOWN1, 7)
g.drive_distance(-60, .6)
u.move_servo(c.FRONT_ARM, c.FA_MID, 7)
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 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)