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 grab_third_pole():
g.turn_with_gyro(-80, 80, 82)
u.move_servo(c.ARM, c.ARM_POLE_GRAB)
u.move_servo(c.HAND, c.HAND_MIDDLE)
g.drive_timed(100, 2100)
msleep(1000)
u.move_servo(c.HAND, c.HAND_CLOSE)
msleep(1000)
# we have the pole
u.move_servo(c.ARM, c.ARM_POLE_PULL)
msleep(500)
g.drive_timed(-100, 1800)
msleep(700)
g.turn_with_gyro(-50, 50, 15)
msleep(1000)
g.drive_timed(100, 500)
msleep(500)
u.move_servo(c.HAND, c.HAND_OPEN)
u.move_servo(c.ARM, c.ARM_POLE_RELEASE + 200)
def deliver_organge_ball():
g.turn_with_gyro(-80, 80, 15)
u.move_servo(c.ARM, c.ARM_SWEEP, 5)
msleep(1000)
g.turn_with_gyro(-80, 80, 60)
u.move_servo(c.ARM, c.ARM_UP, 2)
u.move_servo(c.ARM, c.ARM_SWEEP, 2)
g.turn_with_gyro(80, -80, 60)
u.move_servo(c.ARM, c.ARM_UP, 2)
u.move_servo(c.ARM, c.ARM_SWEEP, 2)
g.turn_with_gyro(-80, 80, 20)
u.move_servo(c.ARM, c.ARM_GRAB)
g.drive_timed(-50, 800) # 1200
u.move_servo(c.HAND, c.HAND_SATELLITE_DROP - 60, 2) # -40
msleep(500)
u.move_servo(c.HAND, c.HAND_SATELLITE_DROP + 75)
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 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 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 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 drop_botguy():
g.turn_with_gyro(-150, 150, 140)
u.move_servo(c.ARM, c.ARM_DROP_BOTGUY, 5)
u.move_servo(c.HAND, c.HAND_OPEN)
def go_to_third_pole():
g.turn_with_gyro(80, -80, 90)
g.drive_timed(175, 2800)
msleep(500)
g.drive_timed(-100, 600)
def go_to_second_pole():
g.turn_with_gyro(80, -80, 90)
g.drive_timed(100, 2600)
g.turn_with_gyro(-80, 80, 85)
g.drive_timed(100, 500)
