def driveToNextTrees2():
print("Driving to Next Set of Trees")
u.move_servo(c.servoArmBin, c.armUp, 25)
mpp.drive_speed(-4.2, 60)
if c.isGreen:
mpp.rotate(-92, 50)
else:
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(5.75, 80)
else:
pass
u.waitForButton()
msleep(500)
if c.isGreen:
mpp.rotate(-95, 50)
mpp.drive_speed(-19.5, 60)
else:
mpp.rotate(-90, 50)
mpp.drive_speed(-20, 80)
u.waitForButton()
mpp.drive_speed(5.5, 80)
if c.isGreen:
mpp.rotate(94.5, 50)
else:
mpp.rotate(93, 50)
msleep(500)
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 init():
u.move_servo(c.servoClaw, c.clawOpen, 100)
u.move_servo(c.servoArm, c.armDown, 100)
u.move_servo(c.outrigger, c.outriggerIn, 10)
#u.move_servo(c.servoCow,c.cowMid,100)
enable_servos()
u.waitForButton()
c.startTime = seconds()
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 calibrate_drive():
#Used to test how straight the robot drives forward and backward
print("Calibration Drive")
if (c.isBlue):
print("IS BLUE")
elif c.isGreen:
print("IS GREEN")
elif c.isYellow:
print("IS YELLOW")
drive_speed(24, 100)
waitForButton()
drive_speed(24, -100)
waitForButton()
def findCanDeliverHome():
print("looking for can")
u.moveServo(c.servoArm, c.armCan, 20)
u.moveServo(c.servoClaw, c.clawOpen90 - 100, 20)
startTime = seconds()
d.lineFollowUntilCan()
totalTime = seconds() - startTime
u.moveServo(c.servoClaw, c.grabCan, 5)
msleep(200)
u.moveServo(c.servoArm, 1200, 10)
msleep(200)
d.rotate(190)
u.waitForButton()
d.smoothTimedLineFollow(totalTime)
def init():
#The starting position is marked with pencil on the closest box on the right side
#If you cant see the lines, the bot should be flush to the wall parallel to the tram
#For a more exact placement, the left wheel should be 4.75 inches from the black tape
#starting positions
print("Init")
if(c.isBlue):
print("IS BLUE")
elif c.isGreen:
print("IS GREEN")
elif c.isYellow:
print("IS YELLOW")
enable_servos()
startTest()
#msleep(500)
u.waitForButton()
c.startTime = seconds()
def init():
#Prime Setup:
#The square up surface on the back of the bot should be flush to the back of the SB
#The left edge of the square up surface should be just to the left of the coupler
#Just use the marks on the table :)
if c.is_clone:
print("Hi! I'm Clone.")
else:
print("Hi! I'm Prime.")
enable_servos()
msleep(500)
print("Don't touch me, I'm calibrating!!!")
g.calibrate_gyro()
msleep(500)
u.move_servo(c.servo_arm, c.arm_drop_off, 10)
# test the motors
d.driveTimed(50, 50, 1000)
d.driveTimed(50, 0, 1000)
d.driveTimed(-50, 0, 1000)
print("testing wrist")
u.move_servo(c.servo_wrist, c.wrist_vertical)
if c.is_prime:
msleep(300)
u.move_servo(c.servo_wrist, c.wristFlipped)
msleep(300)
u.move_servo(c.servo_wrist, c.wrist_horizontal)
print("testing claw")
u.move_servo(c.servo_claw, c.claw_closed)
u.move_servo(c.servo_claw, c.claw_open)
print("testing tophat")
g.drive_condition(50, d.on_black_left, False)
g.drive_condition(50, d.on_black_right, True)
d.drive_to_white_and_square_up(50)
print("testing arm")
u.move_servo(c.servo_arm, c.arm_up, 10)
u.move_servo(c.servo_arm, c.arm_down, 5)
print("place in start posistion")
u.waitForButton()
g.calibrate_gyro()
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)