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 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 drive_to_black_and_square_up(speed):
#msleep(500)
g.drive_condition(speed, on_white_left_and_right,
True) # Drives while neither tophat sees black
print('SAW BLACK!!!', on_black_left(), on_black_right())
d.square_up_black(speed / 2, speed / 2)
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_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 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 init():
#Prime Setup:
#The square up surface on the back of the bot should be flush to the back of the SB
#The left edge of the square up surface should be just to the left of the coupler
#Just use the marks on the table :)
if c.is_clone:
print("Hi! I'm Clone.")
else:
print("Hi! I'm Prime.")
enable_servos()
msleep(500)
print("Don't touch me, I'm calibrating!!!")
g.calibrate_gyro()
msleep(500)
u.move_servo(c.servo_arm, c.arm_drop_off, 10)
# test the motors
d.driveTimed(50, 50, 1000)
d.driveTimed(50, 0, 1000)
d.driveTimed(-50, 0, 1000)
print("testing wrist")
u.move_servo(c.servo_wrist, c.wrist_vertical)
if c.is_prime:
msleep(300)
u.move_servo(c.servo_wrist, c.wristFlipped)
msleep(300)
u.move_servo(c.servo_wrist, c.wrist_horizontal)
print("testing claw")
u.move_servo(c.servo_claw, c.claw_closed)
u.move_servo(c.servo_claw, c.claw_open)
print("testing tophat")
g.drive_condition(50, d.on_black_left, False)
g.drive_condition(50, d.on_black_right, True)
d.drive_to_white_and_square_up(50)
print("testing arm")
u.move_servo(c.servo_arm, c.arm_up, 10)
u.move_servo(c.servo_arm, c.arm_down, 5)
print("place in start posistion")
u.waitForButton()
g.calibrate_gyro()
def power_on_test():
# testing servos
print("testing front arm")
u.move_servo(c.FRONT_ARM, c.FA_UP)
u.move_servo(c.FRONT_ARM, c.FA_MID)
u.move_servo(c.FRONT_ARM, c.FA_UP)
print("testing claw")
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED)
u.move_servo(c.FRONT_CLAW, c.FC_OPEN)
u.move_servo(c.FRONT_CLAW, c.FC_CLOSED)
print("testing left arm")
u.move_servo(c.LEFT_ARM, c.LA_FRONT)
u.move_servo(c.LEFT_ARM, c.LA_BACK)
u.move_servo(c.LEFT_ARM, c.LA_FRONT)
u.move_servo(c.LEFT_ARM, c.LA_BACK)
# testing switch
u.wait_for_switch(c.ARM_SWITCH)
# testing motors
print("testing motors")
g.drive_timed(30, 1000)
# pivoting left and right
g.pivot_on_left_wheel(30, 15)
g.pivot_on_left_wheel(-30, 15)
g.drive_timed(-30, 1000)
# testing tophat
print("testing right tophat")
g.drive_condition(50, d.on_white_right)
msleep(500)
print("see black")
g.drive_condition(50, d.on_black_right)
msleep(500)
print("see white")
g.drive_timed(50, 500)
print("testing left tophat")
g.drive_condition(-50, d.on_white_left)
msleep(500)
print("see black")
g.drive_condition(-50, d.on_black_left)
msleep(500)
print("see white")
g.drive_timed(-50, 500)
print(" ")
print(" ")
print("align robot ")
def drive_till_black_right(speed):
msleep(500)
g.drive_condition(speed, d.on_black_right, False)
msleep(250)
