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)