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 motor_calibration():
move_servo(c.SERVO_ARM, c.ARM_UP)
msleep(1000)
g.calibrate_gyro()
print("Distance calibration:")
print("Place the robot square against some wall or edge, then")
wait_for_button()
g.drive_distance(99, 30)
g.drive_timed(0, 0)
print("Measure distance traveled.")
print(
"If greater than 30 inches, decrease inches-to-ticks value in gyroDrive"
)
print("If less than 30 inches, increase inches-to-ticks value")
print("---")
wait_for_button()
print("Turning calibration:")
print("Orient the robot parallel to a tape line")
wait_for_button()
g.turn_with_gyro(60, -60, 180)
g.drive_timed(0, 0)
msleep(100)
print("If robot overturns, decrease turn_conversion in constants.")
print("If robot underturns, increase turn_conversion in constants.")
DEBUG()
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 get_back_to_coupler():
# g.drive_distance(60, 3)
g.drive_condition(-100, d.on_white_left)
g.turn_with_gyro(-80, 80, 90)
g.drive_distance(-90, 4)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN, 100)
u.move_servo(c.FRONT_ARM, c.FA_COUPLER_DOWN, 100)
g.drive_distance(100, 27)
u.move_servo(c.LEFT_ARM, c.LA_SIDE, 20)
def drop_off_cluster():
global left_burning
print("Dropping off cluster")
g.drive_distance(
90, 14
) # driving towards silver line (tophats land just past silver line, on black)
d.drive_to_white_and_square_up(
90) # square up on white (past black and silver line)
if left_burning == 1:
print("left burning")
g.drive_distance(85, 2.3)
g.turn_with_gyro(-70, 70, 90) # turns and squares up on black
g.drive_distance(80, 4)
else:
print("right burning")
g.turn_with_gyro(0, 90, 60) #wiggles to black line
g.drive_distance(95, 3)
g.turn_with_gyro(90, 0, 60)
if c.is_prime:
d.timed_line_follow_right_smooth(
4.5
) #line follows until there is almost no space between it and the pipe
else:
d.timed_line_follow_right_smooth(4.8)
g.turn_with_gyro(-60, 60, 90) #turns and squares up on black
g.drive_condition(-70, d.on_black_right or d.on_black_left, True)
d.square_up_black(-70, -70)
msleep(50)
g.drive_distance(70, 1)
u.move_servo(c.servo_arm, c.arm_drop_off, 8) #drops off cluster
u.move_servo(c.servo_claw, c.claw_open, 8)
u.move_servo(c.servo_arm, c.arm_drop_off + 200, 5)
u.move_servo(c.servo_arm, c.arm_up, 20)
print("Delivered!")
g.drive_distance(80, 1)
def move_coupler_to_blocks():
print "move coupler"
g.turn_with_gyro(-70, 70, 90)
#square up here
g.drive_distance(-90, 6)
g.drive_condition(90, d.on_white_left)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN, 100)
u.move_servo(c.FRONT_ARM, c.FA_COUPLER_DOWN, 100)
g.drive_distance(90, 21)
# u.move_servo(c.FRONT_ARM, c.FA_KNOCK)
# g.pivot_on_left_wheel(70, 20)
# g.pivot_on_left_wheel(-70, 22)
# d.turn_right_to_line()
# u.move_servo(c. FRONT_ARM, c.FA_COUPLER_DOWN)
#g.drive_distance(90, 14) #13.5
d.set_servo_position(c.LEFT_ARM, c.LA_FRONT)
def deliver_poms():
g.drive_distance(-85, 7)
g.turn_with_gyro(-70, 50, 76)
g.drive_distance(-60, 0.5) # 2
u.move_servo(c.FRONT_ARM, c.FA_CENTER_POMS, 8)
u.move_servo(c.FRONT_ARM, c.FA_DROP_POMS, 8)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN_BIN, 5)
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED, 5)
# u.move_servo(c.FRONT_CLAW, c.FC_OPEN_BIN, 5)
# u.move_servo(c.FRONT_CLAW, c.FC_CLOSED, 5)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN_BIN + 15, 5)
d.drive_timed(-c.WIGGLE_SPEED, c.WIGGLE_SPEED, 175)
d.drive_timed(c.WIGGLE_SPEED, -c.WIGGLE_SPEED, 350)
d.drive_timed(-c.WIGGLE_SPEED, c.WIGGLE_SPEED, 350)
d.drive_timed(c.WIGGLE_SPEED, -c.WIGGLE_SPEED, 350)
d.drive_timed(-c.WIGGLE_SPEED, c.WIGGLE_SPEED, 175)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN, 7)
def pick_up_firetruck():
global left_burning
print("Picking up firetruck")
d.drive_to_white_and_square_up(90)
if left_burning:
print("left burning")
pass
else:
print("right burning")
if c.is_prime:
g.turn_with_gyro(-80, 80, 5)
else:
g.turn_with_gyro(-80, 80, 4)
g.drive_distance(-80, 2.5)
u.move_servo(c.servo_arm, c.arm_down, 15)
g.drive_distance(80, 2.5)
u.move_servo(c.servo_claw, c.claw_closed, 10)
u.move_servo(c.servo_arm, c.arm_up, 12) #picks up firetruck
def get_back_down_from_ramp():
g.turn_with_gyro(80, -80, 80)
u.move_servo(c.LEFT_ARM, c.LA_FRONT, 80)
g.drive_distance(-100, 20)
g.turn_with_gyro(-80, 80, 90)
d.timed_line_follow_left(4000)
g.drive_distance(100, 15)
g.turn_with_gyro(-80, 80, 90)
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 drive_to_valve():
print("driving to valve")
global left_burning
if left_burning:
print("left burning")
g.drive_distance(
80, 6.5) #drives forward a bit after dropping off firetruck
g.turn_with_gyro(70, -70, 90) #turns to face valve
u.move_servo(c.servo_arm, c.arm_valve_grab, 15)
if c.is_prime:
u.move_servo(c.servo_wrist, c.wrist_horizontal, 15)
d.timed_line_follow_left_smooth(2)
g.drive_distance(80, 3.2)
d.timed_line_follow_left_smooth(2.2) #2
else:
g.drive_distance(80, 9)
u.move_servo(c.servo_wrist, c.wrist_horizontal, 15)
d.timed_line_follow_left_smooth(
1.9) #2 # line follows to get in perfect position
msleep(100)
else: #right burning
print("right burning")
g.turn_with_gyro(80, -80, 34)
if c.is_prime:
g.drive_distance(80, 7.1)
else:
g.drive_distance(80, 7.1)
g.turn_with_gyro(-80, 80, 34) # wiggles closer to the line
g.drive_distance(-80, 2)
u.move_servo(c.servo_arm, c.arm_valve_grab, 20)
if c.is_prime:
d.timed_line_follow_left_smooth(1.5)
else:
d.timed_line_follow_left_smooth(
1) # line follows to get in perfect position
g.turn_with_gyro(-80, 80,
5) # turns in a little to grab the valve easier
g.drive_distance(80, .4)
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 drop_off_firetruck():
print("drop off firetruck")
global left_burning
if left_burning == 1:
print("left burning")
d.drive_to_white_and_square_up(80)
g.drive_distance(70, 0.5)
g.turn_with_gyro(60, -60, 90) #90
d.drive_till_black_right(-70)
g.drive_distance(-80, 3) # was 4
u.move_servo(c.servo_arm, c.arm_down, 10) # delivering firetruck
g.turn_with_gyro(50, -50, 10)
u.move_servo(c.servo_claw, c.claw_open, 6)
u.move_servo(c.servo_arm, c.arm_up, 12)
g.turn_with_gyro(-50, 50, 10)
else: # right building on fire
print("right burning")
g.turn_with_gyro(-60, 60, 170)
g.drive_distance(80, 1)
g.turn_with_gyro(-80, 80, 2)
g.drive_distance(90, 2)
u.move_servo(c.servo_arm, c.arm_down, 15) # delivering firetruck
g.turn_with_gyro(-80, 80, 10) # rotates closer to building
u.move_servo(c.servo_claw, c.claw_open, 12)
u.move_servo(c.servo_arm, c.arm_up, 20)
g.turn_with_gyro(80, -80, 10) # rotates back
def return_to_poms():
u.move_servo(c.FRONT_ARM, c.FA_MID)
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED)
g.turn_with_gyro(70, -50, 88)
g.drive_distance(-70, 4)
def go_up_ramp():
d.timed_line_follow_right_smooth(7200)
# at the top of the ramp
g.drive_distance(-90, 2)
g.turn_with_gyro(0, 85, 82)
u.move_servo(c.FRONT_ARM, c.FA_MID)