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