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)
