def grab_second_valve():
print("grabbing second valve")
msleep(100)
g.turn_with_gyro(75, -75, 190) # turns all the way around
u.move_servo(c.servo_wrist, c.wrist_horizontal,
30) # turns wrist horizontally
u.move_servo(c.servo_arm, c.arm_valve_grab, 20)
g.drive_distance(90, 8)
d.drive_to_black_and_square_up(-80)
if c.is_prime:
g.drive_distance(
90,
5) # 7 = inches lego drived towards orange valve before turning
# (worked but got too close to breaking claw on pipe)
g.turn_with_gyro(70, -70, 17) #15
g.drive_distance(85, 4) # 4.1
else:
g.drive_distance(
90,
8) # 7 = inches lego drived towards orange valve before turning
# (worked but got too close to breaking claw on pipe)
g.turn_with_gyro(70, -70, 10) # 15
g.drive_distance(85, 3.4) # 4.1
g.turn_with_gyro(-60, 60, 5)
u.move_servo(c.servo_claw, c.claw_valve, 20)
u.move_servo(c.servo_arm, c.arm_drop_off, 20)
g.drive_distance(70, 0.5)
u.move_servo(c.servo_arm, c.armValve, 20)
u.move_servo(c.servo_wrist, c.wristFlipped,
20) # grabs the second valve and flips it
g.turn_with_gyro(60, -60, 5)
d.drive_to_black_and_square_up(-50) # squares up on the big middle line
def drop_second_valve():
print("dropping off second valve")
g.drive_distance(
-95, 25) # follows the same sequence as the first valve drop off
g.drive_distance(85, 5)
g.turn_with_gyro(80, -80, 90)
d.drive_to_black_and_square_up(80)
d.drive_to_white_and_square_up(80)
g.drive_distance(-50, 3.1)
msleep(100)
g.turn_with_gyro(50, -50, 90)
msleep(100)
if c.is_prime:
g.drive_distance(-50, 4)
else:
g.drive_distance(-50, 4.2) #3.8
msleep(100)
g.turn_with_gyro(30, -30, 25)
u.move_servo(c.servo_arm, c.armValveDrop, 20)
u.move_servo(c.servo_wrist, c.wrist_vertical, 20)
g.turn_with_gyro(-30, 30, 20) #12
g.drive_distance(50, .25)
g.turn_with_gyro(
-30, 30, 10
) # drops off on the same side as the first valve, holds valve in scoring position
# end of lego routine
print("holding the second valve on the pipe")
def go_to_first_pole():
u.move_servo(c.ARM, c.ARM_UP)
d.drive_to_black_and_square_up(100)
g.drive_timed(-100, 1500)
u.move_servo(c.ARM, c.ARM_POLE_GRAB)
g.turn_with_gyro(-80, 80, 90)
u.move_servo(c.HAND, c.HAND_CLOSE)
g.drive_timed(100, 750)
g.turn_with_gyro(-80, 80, 85)
def drive_to_MC():
#Drives towards both medical centers
print("Driving to medical center")
g.pivot_on_left_wheel(90, 90)
u.move_servo(c.servo_arm, c.arm_up)
g.drive_distance(95, 19)
d.drive_to_black_and_square_up(
90) # squaring up on line next to water block
g.drive_distance(-90, 3.5)
g.pivot_on_right_wheel(90, 90) # turn to face silver line
def go_to_organge_ball():
g.turn_with_gyro(80, -80, 20)
d.drive_to_black_and_square_up(-100)
g.drive_condition(100, d.get_bump_left_or_right, False)
g.drive_timed(100, 200)
u.move_servo(c.ARM, c.ARM_POLE_RELEASE + 200)
g.turn_with_gyro(100, -100, 90)
g.drive_timed(100, 1200)
g.drive_timed(-100, 500)
g.turn_with_gyro(80, -80, 150) # 152
def drive_back_to_center():
g.turn_with_gyro(100, -100, 120)
g.drive_timed(100, 1000)
d.drive_to_black_and_square_up(100)
g.turn_with_gyro(-100, 100, 90)
g.drive_timed(200, 3700) # 200, 2000
g.turn_with_gyro(100, -100, 90) # 45
g.drive_timed(100, 1000)
d.drive_to_black_and_square_up(-100)
u.wait_for_button()
g.drive_timed(100, 2800) # 3500
u.wait_for_button()
def grab_pole():
g.drive_timed(100, 2200)
g.turn_with_gyro(-80, 80, 15)
# we have the pipe
u.move_servo(c.ARM, c.ARM_POLE_PULL, 5)
g.drive_timed(-100, 1600)
msleep(100)
g.turn_with_gyro(-50, 50, 15) # score cart
msleep(500)
g.drive_timed(100, 1250)
msleep(500)
u.move_servo(c.ARM, c.ARM_POLE_RELEASE)
g.turn_with_gyro(50, -50, 30)
d.drive_to_black_and_square_up(-100)
u.move_servo(c.ARM, c.ARM_UP)
g.drive_timed(200, 500)
d.drive_to_black_and_square_up(-100)
def first_pole_alt():
u.move_servo(c.ARM, c.ARM_UP)
g.turn_with_gyro(-80, 80, 90)
g.drive_timed(250, 3000)
g.drive_timed(-250, 2300) # 2400
g.turn_with_gyro(-80, 80, 90)
d.drive_to_black_and_square_up(-100)
u.move_servo(c.ARM, c.ARM_POLE_GRAB)
g.drive_timed(100, 1900)
msleep(1000)
u.move_servo(c.HAND, c.HAND_CLOSE)
u.move_servo(c.ARM, c.ARM_POLE_PULL)
msleep(500)
g.drive_timed(-100, 1770)
g.turn_with_gyro(-50, 50, 20) # 15
msleep(500)
g.drive_timed(100, 600)
msleep(500)
u.move_servo(c.HAND, c.HAND_OPEN)
u.move_servo(c.ARM, c.ARM_POLE_RELEASE)
g.turn_with_gyro(50, -50, 20) # 15
def drop_first_valve():
# Places the first valve in its final place
print("dropping off first valve")
g.turn_with_gyro(-70, 70, 90)
d.drive_to_black_and_square_up(
-70) # squares up backward on the long black line across the board
print('drive_to_black... tophat square up completed')
g.drive_distance(-95, 26) # squares up against the wall
print('manual square up completed')
g.drive_distance(85, 5)
g.turn_with_gyro(70, -70, 90)
d.drive_to_black_and_square_up(70)
d.drive_to_white_and_square_up(
70) # squares up on the little line perpendicular to the wall
msleep(100)
g.drive_distance(-50, 3.1)
if c.is_prime:
g.turn_with_gyro(50, -50, 95) # turns to face valve
msleep(100)
g.drive_distance(-50, 4)
else:
g.turn_with_gyro(50, -50, 90)
msleep(100)
g.drive_distance(-50, 4.6) #3.8
msleep(100)
g.turn_with_gyro(
30, -30, 25
) # turns slightly to make sure there is enough space to drop the arm
u.move_servo(c.servo_arm, c.armValveDrop, 20)
u.move_servo(c.servo_wrist, c.wrist_vertical, 20)
g.turn_with_gyro(-30, 30, 20)
g.drive_distance(50, .25)
g.turn_with_gyro(-30, 30, 10)
u.move_servo(c.servo_claw, c.claw_open, 20)
u.move_servo(c.servo_arm, c.arm_drop_off - 100,
20) # slides the valve onto the pipe
print("Delivered!")
g.drive_distance(-90, 7)
def pick_up_valve():
print("picking up valve")
g.turn_with_gyro(-50, 50, 2)
u.move_servo(c.servo_claw, c.claw_valve, 20)
u.move_servo(c.servo_arm, c.arm_drop_off, 20)
g.drive_distance(70, 0.5)
u.move_servo(c.servo_arm, c.armValve, 20)
u.move_servo(
c.servo_wrist, c.wristFlipped,
20) # grabs valve, raises arm, and flips it for a mechanical stop
g.drive_distance(-90, 1)
g.turn_with_gyro(80, -80, 90)
g.drive_distance(-80, 2)
d.drive_to_black_and_square_up(-50)
# msleep(500) # DO NOT REMOVE!
# g.calibrate_gyro()
msleep(100)
# g.turn_with_gyro(50, -50, 2)
if c.is_prime:
g.drive_distance(95, 61) # goes across the board
else:
g.drive_distance(95, 67)
msleep(100)
def second_pole_alt():
g.turn_with_gyro(80, -80, 90)
g.drive_timed(250, 2800)
g.drive_timed(-250, 1000) # 1200
g.turn_with_gyro(-80, 80, 90)
d.drive_to_black_and_square_up(-100)
u.move_servo(c.ARM, c.ARM_POLE_GRAB)
g.drive_timed(100, 1950)
msleep(1000)
u.move_servo(c.HAND, c.HAND_CLOSE)
u.move_servo(c.ARM, c.ARM_POLE_PULL)
msleep(500)
g.drive_timed(-100, 1700)
g.turn_with_gyro(-50, 50, 20) # 15
msleep(500)
g.drive_timed(100, 500)
msleep(500)
u.move_servo(c.HAND, c.HAND_OPEN)
u.move_servo(c.ARM, c.ARM_POLE_RELEASE)
g.turn_with_gyro(50, -50, 20) # 15
u.move_servo(c.ARM, c.ARM_UP)
g.drive_timed(200, 500)
d.drive_to_black_and_square_up(-100)
def drive_to_firetruck():
global left_burning
print("Driving to firetruck")
d.drive_to_black_and_square_up(-90) #squares up on black
if left_burning == 1:
print("left burning routine")
g.drive_distance(90, 3.5)
g.pivot_on_left_wheel(90, 90)
else:
print("right burning routine")
g.drive_distance(90, 4.4) #4.8
g.pivot_on_left_wheel(90, 90)
d.drive_to_black_and_square_up(95)
d.drive_to_white_and_square_up(95)
g.drive_distance(95, 4)
d.drive_to_black_and_square_up(
95
) # True #drives until the black line at the end of the medical center
