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 get_bin(): display("\nFunction: get_bin\n") u.move_servo(c.SERVO_JOINT, c.JOINT_TUCKED, 100) if c.IS_CLONE: x.rotate(-86, 50) else: x.rotate(-86, 50) u.move_servo(c.SERVO_BIN_ARM, c.ARM_APPROACH) u.move_servo(c.SERVO_JOINT, c.JOINT_SWING) msleep(250) if c.IS_CLONE: x.drive_speed(12, 70) else: x.drive_speed(10, 70) u.move_servo(c.SERVO_JOINT, c.JOINT_SWING) u.move_bin(c.ARM_SWING, 5) u.move_servo(c.SERVO_JOINT, c.JOINT_PARALLEL, 5) u.move_bin(c.ARM_APPROACH, 5) u.move_servo(c.SERVO_JOINT, c.JOINT_ROTATE, 5) # x.drive_speed(-20, 100) x.drive_speed(-16, 100) x.drive_speed(-4, 50) if c.HIT_BOTGUY: u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_OUT, 100) x.pivot_right(30, 75) x.pivot_right(-30, 75) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_IN, 100)
def approach_furrow(speed=100, limit_time=3): display("\nFunction: approach_furrow\n") limit = seconds() + limit_time drive_forever(speed, speed) while not on_black_left() and not on_black_right() and seconds() < limit: pass if on_black_right(): drive_forever(speed, 0) display("on right") while not on_black_left() and seconds() < limit: pass drive_forever(0, -speed) while on_black_right() and seconds < limit: pass display("found left") if on_black_left(): display("on left") drive_forever(0, speed) while not on_black_right() and seconds() < limit: pass drive_forever(-speed, 0) while on_black_left() and seconds() < limit: pass display("found right") return seconds() < limit
def approach_furrow2(speed=100, limit_time=3): display("\nFunction: approach_furrow\n") lspeed = speed rspeed = speed limit = seconds() + limit_time drive_forever(lspeed, rspeed) while not on_black_left() and not on_black_right() and seconds() < limit: pass stop() if on_black_left() and on_black_right(): return seconds() < limit if not on_black_right(): display("not on right") drive_forever(0, rspeed) while not on_black_right() and seconds() < limit: pass stop() #drive_forever(0,-speed) #while on_black_right() and seconds < limit: # pass display("aligned right") if not on_black_left(): display("not on left") drive_forever(lspeed, 0) while not on_black_left() and seconds() < limit: pass stop() #drive_forever(-speed, 0) #while on_black_left() and seconds() < limit: # pass display("aligned left") return seconds() < limit
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 go_to_far_side(): display("\nFunction: go_to_far_side\n") display("Long Drive") if c.IS_PRIME: drive_timed(500, 495, 2800) drive_timed(0, 280, 1400) else: drive_timed(500, 495, 2600) #3000 drive_timed(0, 300, 1300) drive_timed(200, 195, 1500) #square up drive_timed(-100, -100, 1250) rotate_degrees(-85, 100) end = seconds() + 7 if c.IS_PRIME: drive_forever(75, 85) else: drive_forever(75, 90) while not on_black_right() and seconds() < end: # or not front_bumped() pass if seconds() >= end: #or front_bumped() drive_timed(-96, -100, 500) stop() drive_timed(-100, -100, 800) if c.IS_PRIME: rotate(110, 1450) else: rotate(110, 1550)
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 wait_for_button(): display("Press Button...") while not digital(c.RIGHT_BUTTON): pass msleep(1) display("Pressed") msleep(1000)
def DEBUG(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() display('Program stop for DEBUG\nSeconds: {}'.format(seconds() - c.startTime)) display("NOTE: {}\t{}".format(seconds(), c.startTime)) exit(0)
def test(): display("\ntest\n") create_disconnect() if not create_connect_once(): display("Create not connected...") exit(0) create_drive_direct(-500, -500) msleep(5000)
def DEBUG_WITH_WAIT(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() msleep(5000) display('Program stop for DEBUG\nSeconds: {}'.format(seconds() - c.startTime)) exit(0)
def main(): display("Hello!") act.init() act.get_out_of_startbox() act.go_to_far_side() act.go_and_drop_poms() act.go_and_dump_blue() if c.seeding: act.hay_grab() act.shutdown()
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 select(): end = seconds() + 3 exit_loop = True state = 0 changed = False begin = False for setting in range(0, 8): if digital(setting): begin = True if begin: display("Starting selection") while not exit_loop or seconds() < end: for setting in range(0, 8): if digital(setting) and setting != state: state = setting changed = True while digital(setting): pass if state != 0: exit_loop = False if changed: display("SELECTION: {}".format(state)) changed = False if right_button(): while right_button(): pass display("Running table setting {}".format(state)) msleep(300) exit_loop = True end = 0 display("Ended selection")
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_and_drop_poms(): display("\nFunction: go_and_drop_poms\n") while not approach_furrow2(100, 4): drive_timed(-100, -100, 2000) rotate(-100, 250) stop() while not approach_furrow2(50, 5): drive_timed(-100, -100, 2000) rotate(-100, 250) stop() drive_timed(50, 50, 400) move_servo(c.SERVO_ARM, c.ARM_DROP, 40) msleep(500) move_servo(c.SERVO_CLAW, c.CLAW_OPEN, 250) msleep(500) move_servo(c.SERVO_ARM, c.ARM_UP, 35) msleep(100)
def rotate_until_stalled(speed, motor): display("\nFunction: rotate_until_stalled\n") counter = 0 motor_power(motor, -speed) previous = abs(get_motor_position_counter(motor)) start = seconds() while counter < 10 and seconds() < start + 5: if abs(get_motor_position_counter(motor)) == previous: counter += 1 else: counter = 0 previous = abs(get_motor_position_counter(motor)) msleep(10) freeze(motor) if seconds() > start + 5: display("Motor was unable to reach stalled position in time.") clear_motor_position_counter(motor)
def go_up_ramp2(): 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(70, 100) else: x.drive_forever(100, 70) msleep(10) x.freeze_motors() u.move_servo(c.SERVO_JOINT, c.JOINT_GROUND) if c.IS_CLONE: x.drive_speed(5, 100) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_ET) x.pivot_right_condition(-30, u.lost_ramp, False) x.pivot_left_condition(-30, u.on_black_back, False) x.linefollow_distance(9) if not c.IS_CLONE: x.drive_speed(8, 100) # 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 init(): display("\nFunction: init\n") create_disconnect() if not create_connect_once(): display("Create not connected...") exit(0) create_full() if c.IS_CLONE: display("I AM CLONE") elif c.IS_PRIME: display("I AM PRIME") else: display("I DON'T KNOW WHAT I AM") set_servo_position(c.SERVO_ARM, c.ARM_DOWN) set_servo_position(c.SERVO_CLAW, c.CLAW_OPEN) set_servo_position(c.SERVO_HAY_ARM, c.HAY_ARM_BARN) set_servo_position(c.SERVO_HAY_SPIN, c.HAY_SPIN_DRIVE) enable_servos() msleep(500) startup_test() move_servo(c.SERVO_HAY_SPIN, c.HAY_SPIN_BARN) move_servo(c.SERVO_HAY_SPIN, c.HAY_SPIN_DRIVE) ''' moved to waiting for light move_servo(c.SERVO_ARM, c.ARM_BACK) #These servo movements allow the create to fit inside the start box move_servo(c.SERVO_HAY_SPIN, c.HAY_SPIN_START) move_servo(c.SERVO_HAY_ARM, c.HAY_ARM_START) infinite_y() ''' # display("Press the left button to run seeding code and right to run head to head code") # while not left_button() or right_button(): # pass # if left_button(): # c.seeding = True # display("seeding") # elif right_button(): # c.seeding = False # display("head to head") wait_4_light() shut_down_in(119) c.START_TIME = seconds()
def go_to_ramp(): display("\nFunction: go_to_ramp\n") u.move_servo(c.SERVO_JOINT, c.JOINT_RAMP_ON) u.move_servo(c.SERVO_JOINT, c.JOINT_ARM_TILT) if c.IS_CLONE: x.rotate(-5, 50) else: x.rotate(-5, 50) x.drive_forever(-50, -50) u.move_bin(c.ARM_TILT, 5) x.drive_speed(-10, 100) x.rotate(5, 65) u.move_servo(c.SERVO_JOINT, c.JOINT_HOLD, 5) u.move_bin(c.ARM_TUCKED, 5) msleep(100) x.drive_speed(-7, 100) x.drive_speed(-6, 75) x.drive_speed(2, 75) x.pivot_right(-90, 60) u.move_servo(c.SERVO_JOINT, c.JOINT_MID)
def move_servo(servo, endPos, speed=10): # speed of 1 is slow # speed of 2000 is fast # speed of 10 is the default now = get_servo_position(servo) if speed == 0: speed = 2047 if endPos > 2040: display("using 2040") endPos = 2040 if endPos < 5: display("using 5") endPos = 5 if now > endPos: speed = -speed for i in range(int(now), int(endPos), int(speed)): set_servo_position(servo, i) msleep(DELAY) set_servo_position(servo, endPos) msleep(DELAY)
def get_out_of_startbox(): display("\nFunction: get_out_of_startbox\n") if c.IS_PRIME: move_servo(c.SERVO_HAY_ARM, c.HAY_ARM_UP) else: move_servo(c.SERVO_HAY_ARM, c.HAY_ARM_START_BOX) move_servo(c.SERVO_HAY_SPIN, c.HAY_SPIN_DRIVE) move_servo(c.SERVO_ARM, c.ARM_DOWN) drive_forever(200, 200) while not on_black_right() and not on_black_left(): pass if on_black_left(): drive_forever(0, 200) while not on_black_right(): pass elif on_black_right(): drive_forever(200, 0) while not on_black_left(): pass while not on_black_right() and not on_black_left(): pass if on_black_left(): drive_forever(0, 200) while not on_black_right(): pass elif on_black_right(): drive_forever(200, 0) while not on_black_left(): pass stop() move_servo(c.SERVO_CLAW, c.CLAW_CLOSE, 100) msleep(100) move_servo(c.SERVO_ARM, c.ARM_UP, 100) drive_timed(200, 200, 650) if c.IS_PRIME: drive_timed(100, 100, 200) rotate_degrees(-85, 100) else: drive_timed(100, 100, 200) rotate_degrees(-76, 100)
def wait_for_someone_to_rotate(motor): display("\nFunction: wait_for_someone_to_rotate\n") display("please spin me back") clear_motor_position_counter(motor) while abs(get_motor_position_counter(motor)) < 350: pass display("good job")
def select2(): selection = 0 if digital(0): display("Started selection\n") display("Set to: {}".format(selection)) while digital(0): pass while not right_button(): if digital(0): while digital(0): pass selection += 1 display("Set to: {}".format(selection)) display("\n Ended selection\n")
def move_bin(armIn, speed=10): # 1263 if armIn < 5: armEnd = 5 elif armIn > 2040: armEnd = 2040 else: armEnd = armIn joint_start = get_servo_position(c.SERVO_JOINT) # 1750 arm_start = get_servo_position(c.SERVO_BIN_ARM) # 700 delta = armEnd - arm_start # 563 for shift in range(0, abs(delta), speed): if delta < 0: shift = -shift # handles negative values (moving bin downwards) set_servo_position(c.SERVO_BIN_ARM, arm_start + shift) set_joint = joint_start + shift if set_joint > 1900: set_joint = 1900 # return elif set_joint < 5: set_joint = 5 # return set_servo_position(c.SERVO_JOINT, set_joint) display("{}\t{}".format(get_servo_position(c.SERVO_JOINT), get_servo_position(c.SERVO_BIN_ARM))) msleep(DELAY) set_joint = joint_start + delta if set_joint > 1900: set_joint = 1900 elif set_joint < 5: set_joint = 5 set_servo_position(c.SERVO_BIN_ARM, armEnd) set_servo_position(c.SERVO_JOINT, set_joint)
def go_and_dump_blue(): display("\nFunction: go_and_dump_blue\n") drive_timed(-100, -100, 2500) #(-400,-400,1000) if c.IS_PRIME: #rotate(-100,2100) rotate_degrees(-123, 100) #rotation to water tank else: rotate(-100, 2100) move_servo(c.SERVO_ARM, c.ARM_DROP, 50) msleep(15000) if c.IS_CLONE: drive_timed(100, 100, 500) y() # wait_for_button(True) msleep(1000) end = seconds() + 6 drive_forever(-200, -200) while not bumped() and seconds() < end: pass if seconds() > end: # wait_for_button(True) drive_timed(150, 150, 500) y_not() drive_forever(-200, -200) end_two = seconds() + 3 display("Seconds: {}\t\tend_two: {}\t\tBumped: {}".format( seconds(), end_two, bumped())) while not bumped() and seconds() < end_two: pass display("Seconds: {}\t\tend_two: {}\t\tBumped: {}".format( seconds(), end_two, bumped())) else: for _ in range(0, 1): stop() msleep(2000) drive_timed(100, 100, 1200) end = seconds() + 6 drive_forever(-200, -200) while not bumped() and seconds() < end: pass stop() msleep(2000) stop()
def drive_till_bump(): display("\nFunction: drive_till_bump\n") if c.IS_CLONE: x.drive_speed(41, 100, True) else: x.drive_speed(42, 100, True)
def self_test(): display("\nFunction: self_test\n") display("Click left button to use botguy hitter else hit right") while not right_button() and not left_button(): pass if right_button(): c.HIT_BOTGUY = False display("wont hit botguy") elif left_button(): c.HIT_BOTGUY = True display("will hit botguy") display("DONE SETTING") if u.on_black_front() or u.on_black_back(): display("Something is wrong with the tophats!") display("LTOPHAT: {}\tRTOPHAT: {}".format(u.on_black_front(), u.on_black_back())) exit(1) while not u.found_bump(): pass display("Good gyro") u.wait_for_button() enable_servos() x.drive_forever(80, 80) x.drive_condition(80, 80, u.on_black_front, False) msleep(500) x.drive_condition(80, 80, u.on_black_back, False) x.freeze_motors() u.move_servo(c.SERVO_JOINT, c.JOINT_MID) u.move_servo(c.SERVO_BIN_ARM, c.ARM_SPINNER_TEST) x.wait_for_someone_to_rotate() u.wait_for_button() x.rotate_until_stalled(20) msleep(500) x.rotate_spinner(.06, -30) msleep(500) x.set_spinner_safe() u.move_servo(c.SERVO_JOINT, c.JOINT_TUCKED) u.move_servo(c.SERVO_BIN_ARM, c.ARM_TUCKED) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_OUT) u.move_servo(c.SERVO_BOT_GUY_HITTER, c.HITTER_IN) msleep(500) x.rotate(15, 60) msleep(1000) x.rotate(-15, 60) display("DONE")
def init(): display("\nFunction: init\n") if c.IS_CLONE: display("I AM CLONE") else: display("I AM PRIME")
def shutdown(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() display('Program stopped\nSeconds: {}'.format(seconds() - c.startTime)) exit(0)