def get_water_cube(): # Drives to cube of water if c.IS_PRIME: g.create_drive_timed(-400, 4) else: g.create_drive_timed(-450, 3.6) g.rotate(-90, 250) g.create_drive_timed(500, 2.1) g.create_drive_timed(250, .2) g.rotate(-85, 300) g.drive_condition(u.on_black_left_tophat, -150, False) m.drive_to_black_and_square_up(100) if c.IS_PRIME: g.rotate(2, 125) else: g.rotate(6, 125) if c.IS_PRIME: g.create_drive_timed(200, .75) g.create_drive_timed(100, .6) else: g.create_drive_timed(200, .75) g.create_drive_timed(100, .3) move_servo(c.sky_arm, c.arm_down, 10) msleep(100) # do not remove move_servo(c.sky_claw, c.claw_closed_water, 10) msleep(100) # do not remoove move_servo(c.sky_arm, c.arm_vertical, 10)
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_to_black_and_square_up(speed): g.drive_condition(black_left_or_right, speed, False) create_drive_direct(0, 0) msleep(500) lspeed = -speed rspeed = -speed while lspeed != 0 or rspeed != 0: if get_black_left(): lspeed = 0 if get_black_right(): rspeed = 0 create_drive_direct(lspeed, rspeed) msleep(100)
def drop_water_cube(): #Create deposits large water cube on burning building g.rotate(70, 400) g.create_drive_timed(400, .5) g.drive_condition(m.on_black_left_tophat, -200, False) g.create_drive_timed(100, 1) g.rotate(-90, 100) g.create_drive_timed(-400, 1.3) g.create_drive_timed(-200, .2) g.rotate(-90, 250) g.create_drive_timed(200, 1.3) m.drive_to_black_and_square_up(-100) if burningSky == 0: print("Left") if c.IS_CLONE: g.rotate(40, 100) else: g.rotate(35, 100) g.create_drive_timed(100, 1.4) move_servo(c.sky_arm, c.arm_low_sky, 10) elif burningSky == 1: print("middle") g.create_drive_timed(100, 1.5) g.create_drive_timed(50, .3) g.rotate(-4, 100) move_servo(c.sky_arm, c.arm_high_sky_deliver - 15, 10) else: print("Right") if c.IS_PRIME: g.rotate(-34, 100) g.create_drive_timed(100, 1) #(50,2.5) g.create_drive_timed(50, 1.2) elif c.IS_CLONE: g.rotate(-42, 100) g.create_drive_timed(200, .25) g.create_drive_timed(50, 1.2) msleep(100) move_servo(c.sky_arm, c.arm_low_sky, 10)
def head_to_elec_lines(): # Goes to electric lines and attatches them print("Heading to electric lines") move_servo(c.sky_arm, c.arm_vertical, 20) if c.IS_PRIME: g.create_drive_timed(-240, .25) else: g.create_drive_timed(-240,.5) g.rotate(-90, 100) u.move_servo(c.electric_arm_base, c.electric_base_up, 20) em.electric_line_motor(30, -600) g.create_drive_timed(500, 3.5) #Square up on wall msleep(100) if not u.get_pipe_switch(): g.create_drive_timed(-200, .65) g.rotate(-90, 300) print(u.method) g.drive_condition(get_bump_or_black, -500, False) print(u.method) if u.method == 1: print("Bumped") g.drive_condition(on_black_left_tophat, 200, False) elif u.method == 2: print("Tophats") elif u.method == 3: print("Create sensors") g.drive_condition(u.bumped and on_black_left_tophat, -200, False) msleep(200) g.drive_condition(on_black_left_tophat, 200, False) else: print("None (shouldn't happen)") g.create_drive_timed(-200, 1) m.drive_to_black_and_square_up(125) g.create_drive_timed(-120, .25) g.rotate(90, 125) g.create_drive_timed(125, 2.5) #Square up on wall if not u.get_pipe_switch(): g.create_drive_timed(-250, .5) g.create_drive_timed(500, 1) msleep(300) else: msleep(11000)
def init(): #Test to make sure all the moving parts and sensors work the way they should global first if c.IS_PRIME: print("I are prime") if c.IS_CLONE: print("I are clone") #SETUP #Arm pointing towards the medical centers #Edges of create line up with the black tape intersection #Align square up surface with left side of middle bump #This should ensure that the robot is pointing directly towards the middle bump (DRS forward) print("Starting init") print("Enabling my servos") enable_servos() print("Camera Init") p.camera_init() p.camera_update() msleep(500) print("Testing Servos") msleep(500) msleep(500) move_servo(c.sky_arm, c.arm_vertical) msleep(500) move_servo(c.sky_claw, c.claw_open) msleep(500) move_servo(c.sky_claw, c.claw_closed_water) msleep(500) u.move_servo(c.electric_arm_base, c.electric_base_right) em.clear_ticks_button() u.move_servo(c.electric_arm_base, c.electric_base_down) print("Connecting to Create") create_connect() create_full() #turnCalibration() print("Drive and Sensor Testing") g.rotate(-56, 150) msleep(500) p.find_burning_sky() done = seconds() + 3 first = False print("Waiting for you to press the switch and check which building is burning") while not u.get_pipe_switch(): pass g.rotate(56, 150) msleep(500) m.drive_to_black_and_square_up(-200) g.drive_condition(on_black_left_tophat, -250, False) print("Setting servos for the run") move_servo(c.sky_arm, c.arm_start) msleep(500) move_servo(c.sky_claw, c.claw_open) msleep(500) u.wait_for_button_camera() c.START_TIME = seconds() shut_down_in(119.5) print(k.camera_reads) if u.compute_burning_MC() == False: #burning MC is on right print("Pushing switch") msleep(300) u.move_servo(c.sky_arm, c.arm_moving) u.move_servo(c.sky_arm, c.arm_button, 5) msleep(100) u.move_servo(c.sky_arm, c.arm_vertical) else: #burning MC is on left print("Not pushing switch") u.move_servo(c.sky_arm, c.arm_vertical) msleep(1000) msleep(500) g.calibrate_gyro() u.move_servo(c.electric_arm_base, c.electric_base_left)