def driveToNextTrees2():
print("Driving to Next Set of Trees")
u.move_servo(c.servoArmBin, c.armUp, 25)
mpp.drive_speed(-4.2, 60)
if c.isGreen:
mpp.rotate(-92, 50)
else:
mpp.rotate(-90, 50)
mpp.drive_speed(-7, 80)
msleep(500)
mpp.drive_speed(17, 80) # 14
msleep(500)
mpp.rotate(89, 50)
msleep(1000)
if c.isGreen:
u.smoothLineFollowLeft(5.75, 80)
else:
pass
u.waitForButton()
msleep(500)
if c.isGreen:
mpp.rotate(-95, 50)
mpp.drive_speed(-19.5, 60)
else:
mpp.rotate(-90, 50)
mpp.drive_speed(-20, 80)
u.waitForButton()
mpp.drive_speed(5.5, 80)
if c.isGreen:
mpp.rotate(94.5, 50)
else:
mpp.rotate(93, 50)
msleep(500)
def getOutAndGrabPoms():
# x.drive_speed(6, 75)
x._drive(100,100)
msleep(750)
x._drive(0,0)
u.move_servo(c.servoClaw, c.clawClosed, 35)
u.move_servo(c.servoArm, c.armUp, 100)
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 go_up_ramp():
display("\nFunction: go_up_ramp\n")
u.move_bin(c.ARM_SWING)
x.drive_speed(12, 100)
start_time = seconds()
x.drive_speed(5, 100)
while gyro_y() < 100 or seconds() < start_time + 2:
if u.on_black_front():
x.drive_forever(50, 100)
else:
x.drive_forever(100, 50)
msleep(10)
x.drive_speed(8, 100)
u.move_servo(c.SERVO_JOINT, c.JOINT_GROUND)
# u.wait_for_button()
print("1")
x.pivot_left_condition(30, u.on_black_front, False)
# u.wait_for_button()
print("2")
# if u.on_black_back():
x.pivot_right_condition(30, u.on_black_back)
# x.pivot_right(35, 30)
# u.wait_for_button()
print("3")
x.pivot_right_condition(30, u.on_black_back, False)
# u.wait_for_button()
print("4")
x.pivot_left_condition(30, u.on_black_front, False)
# u.wait_for_button()
print("5")
u.move_bin(c.ARM_ALL_UP)
msleep(500)
def driveToNextTrees():
#Uses an amazingly smooth line follow to go from one set of trees to the other
print("Driving to Next Set of Trees")
u.move_servo(c.servoArmBin, c.armUp, 25)
mpp.drive_speed(-4.7, 60)
mpp.rotate(-90, 50)
mpp.drive_speed(-7, 80)
msleep(500)
mpp.drive_speed(17, 80)#14
msleep(500)
mpp.rotate(89, 50)
msleep(1000)
if c.isGreen:
u.smoothLineFollowLeft(7.2, 80)
elif c.isBlue:
u.smoothLineFollowLeft(7.5, 80)
elif c.isYellow:
u.smoothLineFollowLeft(5, 80)
#mpp.drive_speed(44, 90)
msleep(500)
if c.isGreen:
mpp.rotate(-95, 50)
else:
mpp.rotate(-90, 50)
mpp.drive_speed(-20, 80)
msleep(500)
mpp.drive_speed(5.5, 80)
if c.isGreen:
mpp.rotate(94.5, 50)
elif c.isBlue:
mpp.rotate(93, 50)
elif c.isYellow:
mpp.rotate(93, 50)
msleep(500)
def go_and_score_the_bin():
display("\nFunction: go_and_score_the_bin\n")
u.move_servo(c.SERVO_JOINT, c.JOINT_DELIVER, 4)
msleep(500)
u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_OUT, 100)
# x.linefollow_distance(28, 50, 70)
x.linefollow_distance(21, 50, 70, 5)
x.pivot_right(-32.5, 50)
# x.drive_speed(-2, 50)
# x.rotate(-10, 50)
# x.drive_speed(2.5, 50)
# x.pivot_right(40, 50)
# x.drive_speed(2.5, 50)
# u.wait_for_button()
if not c.IS_CLONE:
x.drive_speed(1, 50)
disable_servo(c.SERVO_JOINT)
msleep(500)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_MAX)
msleep(500)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_HIT, 20)
msleep(300)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_MAX, 20)
x.drive_speed(1, 50)
x.pivot_right(30, 50)
def drive_and_servo():
drive = threading.Thread(name = 'deamon', target = lambda: drive_distance(100,100))
drive.start()
while True:
u.move_servo(c.servo_arm, c.arm_down, 10)
msleep(500)
u.move_servo(c.servo_arm, c.arm_up, 10)
msleep(500)
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 alt_init():
u.move_servo(c.SERVO_JOINT, c.JOINT_MID, 2047)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED, 2047)
enable_servos()
u.wait_for_button()
# u.move_bin(c.ARM_SWING)
# u.move_bin(c.ARM_ALL_UP)
x.drive_speed(70, 100)
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 grabCowAndGo():
u.move_servo(c.servoCowClaw, c.cowClawOpen, 10)
msleep(500)
#u.move_servo(c.servoCow, c.cowDown, 10)
msleep(1000)
u.move_servo(c.servoCowClaw, c.cowClawClose, 10)
u.waitForButton()
#u.move_servo(c.servoCow, c.cowUp,10)
msleep(300)
x.drive_speed(34, 100)
def driveUntilTree():
#Helper function for driveFirstThreeTrees()
#Drives from one date tree to the next
print("Looking for Trees")
sec = seconds() + 5
while analog(c.ET) < c.onTree and seconds() < sec:
mpp.drive_speed(.05, 90)
print(analog(c.ET))
print("Saw Tree")
u.move_servo(c.servoArmBin, c.armDown, 20)
def test():
print"test"
if c.isClone:
print "I am Clone"
else:
print "I am Prime"
enable_servos
u.move_servo(c.servoArm, c.armUp)
msleep(200)
x.drive_speed(50,100)
u.DEBUG()
def start():
display("\nFunction: start\n")
u.wait_4_light(ignore=False)
if c.IS_CLONE:
msleep(2500)
else:
msleep(2000)
shut_down_in(119.75)
c.startTime = seconds()
display("NOTE: {}\t{}".format(seconds(), c.startTime))
u.move_servo(c.SERVO_JOINT, c.JOINT_TUCKED)
enable_servo(c.SERVO_JOINT)
def new_get_poms_timed(speed, time):
for x in range(3):
get_poms_wiggle(speed, time / 3)
motor(c.DATEMOTOR1, 0)
motor(c.DATEMOTOR2, 0)
u.move_servo(c.servoDateWheel, c.wheelIn + 300, 20)
u.move_servo(c.servoDateWheel, c.wheelIn, 20)
pivot_right(1, 60)
pivot_left(.5, 60)
mpp.drive_speed(-.25, 60)
motor(c.LMOTOR, 0)
motor(c.RMOTOR, 0)
def get_can_camera(color):
while True:
u.move_servo(c.servo_claw, c.claw_open, 10)
msleep(500)
u.move_servo(c.servo_arm, c.arm_down, 10)
msleep(500)
camera.get_can(color)
m.drive_timed(-50, -50, 2100)
m.drive_timed(50, -50, 2100)
camera_update()
msleep(750)
print(get_object_count(color))
v.first_time = False
def getThirdDateBin():
mpp.drive_speed(5.8, 100) #5.8
u.move_servo(c.servoPipeWheel, c.pipeStraight, 25)
mpp.rotate(95, 75) #101
mpp.drive_speed(1, 80) #2
u.move_servo(c.servoPipeWheel, 900, 20)
mpp.drive_timed(-95, -60, 2) #was -85
tim = seconds()
while analog(c.ET) < (c.onTree - 750) and seconds() - tim < 2.8:
mpp.drive_timed(-95, 60, .05) #was -85, -60
if seconds() - tim > 2.8:
print("Fourth tree timeout")
mpp.rotate(16, 80)
mpp.drive_speed(-.6, 70)
mpp.pivot_right(15, 65)
u.move_servo(c.servoPipeWheel, c.pipeBin, 25)
print "Saw Tree"
mpp.pivot_right(6, 75)
mpp.drive_speed(1.5, 80)
mpp.rotate(-10, 80) #-20
else:
mpp.pivot_right(15, 65)
u.move_servo(c.servoPipeWheel, c.pipeBin, 25)
print "Saw Tree"
mpp.pivot_right(6, 75)
mpp.drive_speed(1.5, 80)
mpp.rotate(-20, 80) # -18
mpp.drive_speed(-4.5, 80) #-6.5
mpp.pivot_left(-25, 80)
msleep(100)
mpp.pivot_right(25, 75) #was 18 degrees
mpp.new_get_poms_timed(100, 8000) #6000
mpp.pivot_right(-3, 60)
def driveFirstTreesExperiment():
if c.isGreen:
mpp.drive_speed(-7, 50)
mpp.drive_speed(0.5, 20) # .73
msleep(250)
#motor_power(c.RMOTOR, 10)
mpp.get_poms_timed(70, 6000)
msleep(1000)
driveUntilTree()
u.move_servo(c.servoArmBin, c.armDown)
u.waitForButton()
mpp.get_poms_timed(70, 6000)
msleep(1000)
driveUntilTree()
mpp.get_poms_timed(70, 6000)
msleep(1000)
elif c.isBlue:
mpp.drive_speed(-7, 50)
mpp.drive_speed(0.5, 20) # .73
msleep(250)
# motor_power(c.RMOTOR, 10)
mpp.get_poms_timed(50, 9000)
msleep(1000)
u.move_servo(c.servoPipeWheel,c.pipeStraight)
u.waitForButton()
driveUntilTree()
mpp.drive_speed(2.85,50)
u.move_servo(c.servoPipeWheel,c.pipeBin)
u.move_servo(c.servoArmBin, c.armDown)
mpp.get_poms_timed(50, 9000)
msleep(1000)
driveUntilTree()
mpp.get_poms_timed(60, 6000)
msleep(1000)
def leave_startbox():
display("\nFunction: leave_startbox\n")
u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED)
x.drive_condition(80, 80, u.on_black_front, False)
x.drive_speed(-4, 60)
if c.IS_CLONE:
x.rotate(-92, 70)
else:
x.rotate(-96, 70)
x.drive_speed(-34, 100)
x.drive_condition(80, 80, u.on_black_front, False)
x.drive_speed(1, 80)
x.rotate(92, 60)
x.drive_speed(-7, 85)
def go_to_spinner():
display("\nFunction: go_to_spinner\n")
u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED, 5)
if c.IS_CLONE:
x.drive_speed(8, 100)
else:
x.drive_speed(11, 100)
if c.IS_CLONE:
x.pivot_left(-90, 70)
else:
x.pivot_left(-88, 70)
x.drive_speed(22, -100, True)
x.pivot_left(-32, 50)
x.drive_speed(-11, 80)
x.pivot_right(-32, 50)
x.drive_speed(-3, 70)
x.drive_condition(50, 50, u.on_black_front, False)
if c.IS_CLONE:
x.rotate(90, 35)
else:
x.rotate(98, 35)
u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED)
u.move_servo(c.SERVO_JOINT, c.JOINT_PARALLEL)
x.drive_condition(80, 80, u.on_black_front, False)
x.drive_condition(50, 50, u.on_black_front, True)
x.rotate_spinner(.25, 80)
x.drive_speed(5, 60)
u.move_servo(c.SERVO_JOINT, c.JOINT_GROUND)
x.rotate_spinner(4, -70)
x.rotate_to_safe(50)
def seeBlocks():
s = p.checkColor(colorOrder)
if s == c.RED:
print("found red")
elif s == c.YELLOW:
print("found yellow")
elif s == c.GREEN:
print("found green")
else:
print("Did not find cube")
if colorOrder[0] == 3:
u.move_servo(c.servoFrisbeeArm, c.frisbeeArmStartPosition + 250)
msleep(300)
u.move_servo(c.servoFrisbeeArm, c.frisbeeArmStartPosition)
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 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 startTest():
print("Running Start Test")
mpp.drive_speed(3, 30)
mpp.drive_speed(-3, 30)
u.move_servo(c.servoArmBin, c.armUp)
u.move_servo(c.servoArmBin, c.armDown)
u.move_servo(c.servoPipeWheel, c.pipeOut)
u.move_servo(c.servoPipeWheel, c.pipeBin)
mpp.drive_date_motor(50, 1000)
def get_soda_can():
u.wait_4_button()
u.move_servo(c.servo_arm, c.arm_down, 10)
u.move_servo(c.servo_claw, c.claw_open, 10)
msleep(1000)
m.drive_timed(100, 100, 1000)
msleep(500)
u.move_servo(c.servo_claw, c.claw_closed, 10)
u.move_servo(c.servo_arm, c.arm_up, 10)
def get_can_even_better(): #line follow on the curved line and grab the can
u.move_servo(c.servo_arm, c.arm_down, 10)
msleep(100)
u.move_servo(c.servo_claw, c.claw_open, 10)
u.line_follow_amazing()
m.drive_timed(50, 50, 1000)
u.move_servo(c.servo_claw, c.claw_closed, 10)
msleep(100)
u.move_servo(c.servo_arm, c.arm_up, 10)
def driveFirstTreesH2H():
print "driveFirstTrees"
mpp.drive_speed(-3.3, 100) #-9
msleep(100)
mpp.pivot_left(-40, 70)
msleep(100)
mpp.drive_speed(-2, 70)
mpp.drive_speed(.5, 70) # .73, 35
msleep(100)
mpp.pivot_right(8, 80) #8
u.move_servo(c.servoDateWheel, c.wheelIn + 100, 20)
u.move_servo(c.servoDateWheel, c.wheelIn, 8)
mpp.new_get_poms_timed(100, 5000)
msleep(100)
u.move_servo(c.servoDateWheel, c.wheelOut, 20)
u.move_servo(c.servoArmBin, c.armDown, 20)
u.move_servo(c.servoPipeWheel, c.pipeBin, 20)
def upRamp():
x.rotate(25, 50)
x.drive_speed(36, 100)
x.drive_speed(6, -50)
x.rotate(89, 50)
x.drive_speed(30, 100)
x.drive_speed(4, -50)
x.rotate(79, 50)
u.move_servo(c.servoArm, c.armBotguy,10)
msleep(300)
x.drive_speed(70, 75)
u.move_servo(c.servoCow, c.cowDown, 10)
msleep(300)
x.rotate(-45, 50)
print "Seconds elapsed: " + seconds() - c.startTime
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 get_can(color): # Works pretty well but could be better
print("Searching for cans")
camera_update()
msleep(100)
camera_areas = [0, 0, 0, 0, 0]
area_average = get_object_area(color, 0)
print(get_object_count(color))
while area_average < 1400 or v.first_time is False:
v.first_time = True
print("Found a can")
camera_update()
if len(camera_areas) == 5:
camera_areas.pop(0)
camera_areas.append(get_object_area(color, 0))
area_average = sum(camera_areas) / len(camera_areas)
# Splits the cameras view into 5 sections so the robot can change the angle of its turns depending on which of the five zones red is in
if get_object_count(c.CHANNEL_RED) > 0:
if get_object_center_x(color, 0) < 40:
motor(c.LEFT_MOTOR, 0)
motor(c.RIGHT_MOTOR, 40)
elif get_object_center_x(color, 0) > 120:
motor(c.LEFT_MOTOR, 40)
motor(c.RIGHT_MOTOR, 0)
elif get_object_center_x(color, 0) > 40 and get_object_center_x(
color, 0) < 70:
motor(c.LEFT_MOTOR, 20)
motor(c.RIGHT_MOTOR, 40)
elif get_object_center_x(color, 0) > 90 and get_object_center_x(
color, 0) < 120:
motor(c.LEFT_MOTOR, 40)
motor(c.RIGHT_MOTOR, 20)
else:
motor(c.LEFT_MOTOR, 40)
motor(c.RIGHT_MOTOR, 40)
else:
m.drive_timed(20, -20, 1)
print(area_average)
# Once the can is close enough this code will pick it up then find a board edge and drop the can
m.drive_timed(48, 50, 2800)
msleep(500)
u.move_servo(c.servo_claw, c.claw_closed, 10)
msleep(500)
u.move_servo(c.servo_arm, c.arm_up, 10)
msleep(500)
m.drive_timed(-50, 50, 2100)
while gyro_y(
) > -150: # You've got a basic version of bumpy. Can you tell it to pay attention to
m.drive_timed(
70, 70, 1
) # all axis? Brief bit of math: distance formula. Math.sqrt, Math.pow, etc.
u.move_servo(c.servo_arm, c.arm_up, 10)
msleep(500)
u.move_servo(c.servo_claw, c.claw_open, 10)
msleep(500)
