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 drive_to_white_and_square_up(speed):
#msleep(500)
g.drive_condition(50, d.on_black_right and d.on_black_left, True)
g.drive_distance(50, 0.5)
d.square_up_black(-50, -50)
g.drive_distance(50, 0.25)
msleep(250)
def lower_ramp():
g.drive_distance(100, 23) # 75
msleep(100)
set_servo_position(c.LEFT_ARM, c.LA_SIDE) # grab coupler
g.drive_condition(-80, d.on_white_left) # -60
msleep(100)
d.square_up_black(-60, -60)
msleep(100)
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 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 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 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 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 grab_cluster():
global left_burning
print("Waiting for something to press button")
done = seconds() + 2.0
while seconds(
) < done: #waiting for Create to send MC order (which building is on fire)
if digital(c.BUTTON) == 1:
left_burning = 0
msleep(10)
if left_burning == 1:
print("The burning medical center is on the LEFT")
else:
print("The burning medical center is on the RIGHT")
print("Grabbing cluster")
g.drive_timed(50, 0.55)
u.move_servo(c.servo_claw, c.claw_closed, 12)
u.move_servo(c.servo_arm, c.arm_valve_grab, 15)
g.drive_distance(80, 2)
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 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 back_to_up_ramp_position():
u.move_servo(c.LEFT_ARM, c.LA_BACK, 100)
u.move_servo(c.FRONT_ARM, c.FA_UP, 100)
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED, 100)
g.pivot_on_right_wheel(80, 92)
g.drive_distance(100, 22)
g.drive_distance(-80, 4)
g.pivot_on_left_wheel(80, 90) # 90
g.drive_distance(90, 2)
set_servo_position(c.LEFT_ARM, c.LA_RAMP_SIDE) # fold in left arm
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
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 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 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 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 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 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 go_to_other_side():
g.drive_distance(90, 40)
msleep(50)
def return_to_start():
g.drive_distance(-90, 25)
msleep(50)
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 back_up_around_blocks():
g.drive_distance(-80, 3)
g.pivot_on_left_wheel(-70, 25)
g.drive_distance(-80, 19) # back up
g.pivot_on_left_wheel(70, 25)
g.drive_distance(-70, 12)
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)
