def do_magnets():
g.backwards_gyro_until_black_rcliff()
g.forwards_gyro(100)
m.lift_magnet_arm()
s.align_on_wall_right()
s.wfollow_right_smooth_until_black_rcliff()
g.backwards_gyro_until_white_rcliff(0)
msleep(1000)
g.backwards_gyro(50)
m.lower_magnet_arm()
g.backwards_gyro_until_black_lfcliff()
s.align_far_fcliffs()
g.backwards_gyro(80)
m.lift_magnet_arm()
g.backwards_gyro(1500)
msleep(1000)
g.forwards_gyro(50)
m.lower_magnet_arm()
def get_left_coupler():
# Gets to the bottom left square
g.forwards_gyro_through_line_rfcliff()
s.align_far_fcliffs()
g.forwards_gyro_until_white_rfcliff()
s.align_on_wall_left()
s.wfollow_left_smooth_until_white_rfcliff(0)
s.wfollow_left_smooth_until_black_rfcliff()
s.align_close_fcliffs()
# Turns right and goes to top left coupler
g.turn_right_gyro(85)
g.forwards_gyro_through_line_rfcliff()
s.lfollow_lfcliff_until_bump_pid(bias=15)
g.backwards_gyro(100)
s.turn_right_until_rfcliff_senses_white(0)
msleep(1500)
s.turn_right_until_rfcliff_senses_black(c.RIGHT_TURN_TIME * 6)
s.lfollow_rfcliff_until_lfcliff_senses_black_pid()
m.lower_arm()
g.turn_left_gyro(23)
g.backwards_gyro(1125)
#g.backwards_gyro_until_item_is_in_claw(1100)
pick_up_coupler()
def get_gas_valve():
s.wfollow_right_until_black_left_front()
s.wfollow_right_until_white_left_front()
s.wfollow_right_until_black_left_front()
s.wfollow_right_until_black_right()
s.wfollow_right_until_white_right()
s.turn_left_until_rfcliff_senses_white()
s.turn_left_until_rfcliff_senses_black()
m.turn_left(c.LEFT_TURN_TIME / 4.8)
# This turns the robot closer to 90 degrees
s.forwards_until_white_lfcliff()
s.forwards_through_line_lfcliff()
s.align_far_fcliffs()
s.align_far_fcliffs()
s.forwards_until_bump()
m.backwards(1000)
#g.turn_left_gyro(180)
m.turn_left()
m.turn_left()
s.forwards_until_black_lfcliff()
s.align_close_fcliffs()
s.align_close_fcliffs()
#m.turn_left(250)
m.lower_arm()
msleep(500)
m.backwards(900)
m.lift_arm(3, 1, c.ARM_HALF_UP_POS)
m.backwards(300)
m.lift_arm()
s.forwards_until_black_lfcliff()
s.align_close_fcliffs()
s.forwards_until_bump()
m.backwards(100)
#g.turn_right_gyro()
m.turn_right(c.RIGHT_TURN_TIME / 1.2)
s.wfollow_left_until_white_right_front(9999)
s.wfollow_left_until_black_right_front(9999)
s.wfollow_left_until_white_right_front(9999)
s.align_far_fcliffs()
s.align_far_fcliffs()
m.backwards(500)
#g.turn_right_gyro(30)
m.turn_right(c.RIGHT_TURN_TIME / 3.5)
m.backwards(400)
m.lower_arm(1, 1, c.ARM_DELIVERY_POS)
shut_down_in(118)
def calibrate_with_gyro_angle_calibration():
# Initialize variables.
c.MIN_SENSOR_VALUE_LCLIFF = 90000
c.MAX_SENSOR_VALUE_LCLIFF = 0
c.MAX_SENSOR_VALUE_RCLIFF = 0
c.MIN_SENSOR_VALUE_RCLIFF = 90000
c.MAX_SENSOR_VALUE_LFCLIFF = 0
c.MIN_SENSOR_VALUE_LFCLIFF = 90000
c.MAX_SENSOR_VALUE_RFCLIFF = 0
c.MIN_SENSOR_VALUE_RFCLIFF = 90000
angle = 0
error = 0
total_left_speed = 0
total_right_speed = 0
run_throughs = 0
sec = seconds() + 3
print "Running calibrate()"
m.activate_motors(int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2))
while seconds() < sec:
if get_create_lcliff_amt() > c.MAX_SENSOR_VALUE_LCLIFF:
c.MAX_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt()
if get_create_lcliff_amt() < c.MIN_SENSOR_VALUE_LCLIFF:
c.MIN_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt()
if get_create_rcliff_amt() > c.MAX_SENSOR_VALUE_RCLIFF:
c.MAX_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt()
if get_create_rcliff_amt() < c.MIN_SENSOR_VALUE_RCLIFF:
c.MIN_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt()
if get_create_lfcliff_amt() > c.MAX_SENSOR_VALUE_LFCLIFF:
c.MAX_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt()
if get_create_lfcliff_amt() < c.MIN_SENSOR_VALUE_LFCLIFF:
c.MIN_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt()
if get_create_rfcliff_amt() > c.MAX_SENSOR_VALUE_RFCLIFF:
c.MAX_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt()
if get_create_rfcliff_amt() < c.MIN_SENSOR_VALUE_RFCLIFF:
c.MIN_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt()
left_speed = int(c.BASE_LM_POWER / 2) + error
right_speed = int(c.BASE_RM_POWER / 2) - error
m.activate_motors(left_speed, right_speed)
total_left_speed += left_speed
total_right_speed += right_speed
run_throughs += 1
msleep(10)
angle += (gyro_z() - g.bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
m.deactivate_motors()
c.LCLIFF_BW = (
(c.MAX_SENSOR_VALUE_LCLIFF + c.MIN_SENSOR_VALUE_LCLIFF) / 2) + 100
c.RCLIFF_BW = (
(c.MAX_SENSOR_VALUE_RCLIFF + c.MIN_SENSOR_VALUE_RCLIFF) / 2) + 100
c.LFCLIFF_BW = (
(c.MAX_SENSOR_VALUE_LFCLIFF + c.MIN_SENSOR_VALUE_LFCLIFF) / 2) + 100
c.RFCLIFF_BW = (
(c.MAX_SENSOR_VALUE_RFCLIFF + c.MIN_SENSOR_VALUE_RFCLIFF) / 2) + 200
c.BASE_LM_POWER = int((total_left_speed * 2) / run_throughs)
c.BASE_RM_POWER = int((total_right_speed * 2) / run_throughs)
c.FULL_LM_POWER = c.BASE_LM_POWER
c.FULL_RM_POWER = c.BASE_RM_POWER
c.HALF_LM_POWER = int(c.BASE_LM_POWER) / 2
c.HALF_RM_POWER = int(c.BASE_RM_POWER) / 2
print "LCLIFF_BW: " + str(c.LCLIFF_BW)
print "RCLIFF_BW: " + str(c.RCLIFF_BW)
print "LFCLIFF_BW: " + str(c.LFCLIFF_BW)
print "RFCLIFF_BW: " + str(c.RFCLIFF_BW)
print "BASE_LM_POWER: " + str(c.BASE_LM_POWER)
print "BASE_RM_POWER: " + str(c.BASE_RM_POWER)
msleep(100)
s.backwards_until_black_cliffs()
s.align_far_cliffs()
s.turn_left_until_lfcliff_senses_black(0)
g.determine_gyro_conversion_rate()
msleep(100)
g.turn_right_gyro(45)
s.backwards_until_black_lfcliff()
s.align_far_fcliffs()
s.backwards_through_line_lfcliff()
s.align_close_fcliffs()
m.backwards(200)
msleep(300)
ao()
# DON'T DELETE THESE NEXT 4 LINES. They are purposeful. It avoids the roomba going into sleep mode after the calibration and not starting right.
create_disconnect()
msleep(4000)
#wait_for_light(c.LIGHT_SENSOR)
create_connect()
shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
def calibrate():
max_sensor_value_rcliff = 0
min_sensor_value_rcliff = 90000
max_sensor_value_lcliff = 0
min_sensor_value_lcliff = 90000
max_sensor_value_lfcliff = 0
min_sensor_value_lfcliff = 90000
max_sensor_value_rfcliff = 0
min_sensor_value_rfcliff = 90000
if c.BASE_LM_POWER == 0:
print "c.BASE_LM_POWER can not equal 0 for the calibrate command. Autosetting to 109."
c.BASE_LM_POWER = 109
sec = seconds() + (4 * 109 / c.BASE_LM_POWER)
print "Running calibrate()"
m.activate_motors(int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2))
print str(int(c.BASE_LM_POWER / 2))
print str(int(c.BASE_RM_POWER / 2))
while seconds() < sec:
if get_create_rcliff_amt() > max_sensor_value_rcliff:
max_sensor_value_rcliff = get_create_rcliff_amt()
if get_create_rcliff_amt() < min_sensor_value_rcliff:
min_sensor_value_rcliff = get_create_rcliff_amt()
if get_create_lcliff_amt() > max_sensor_value_lcliff:
max_sensor_value_lcliff = get_create_lcliff_amt()
if get_create_lcliff_amt() < min_sensor_value_lcliff:
min_sensor_value_lcliff = get_create_lcliff_amt()
if get_create_rfcliff_amt() > max_sensor_value_rfcliff:
max_sensor_value_rfcliff = get_create_rfcliff_amt()
if get_create_rfcliff_amt() < min_sensor_value_rfcliff:
min_sensor_value_rfcliff = get_create_rfcliff_amt()
if get_create_lfcliff_amt() > max_sensor_value_lfcliff:
max_sensor_value_lfcliff = get_create_lfcliff_amt()
if get_create_lfcliff_amt() < min_sensor_value_lfcliff:
min_sensor_value_lfcliff = get_create_lfcliff_amt()
msleep(1)
m.deactivate_motors()
c.LCLIFF_BW = (
(max_sensor_value_lcliff + min_sensor_value_lcliff) / 2) + 500
c.RCLIFF_BW = (
(max_sensor_value_rcliff + min_sensor_value_rcliff) / 2) + 500
c.LFCLIFF_BW = (
(max_sensor_value_lfcliff + min_sensor_value_lfcliff) / 2) + 500
c.RFCLIFF_BW = (
(max_sensor_value_rfcliff + min_sensor_value_rfcliff) / 2) + 500
print "LCLIFF_BW: " + str(c.LCLIFF_BW)
print "RCLIFF_BW: " + str(c.RCLIFF_BW)
print "LFCLIFF_BW: " + str(c.LFCLIFF_BW)
print "RFCLIFF_BW: " + str(c.RFCLIFF_BW)
print "max_sensor_value_rcliff: " + str(max_sensor_value_rcliff)
print "min_sensor_value_rcliff: " + str(min_sensor_value_rcliff)
msleep(500)
s.backwards_until_black_cliffs()
s.align_far_cliffs()
s.turn_left_until_lfcliff_senses_black()
msleep(300)
g.calibrate_gyro_degrees()
msleep(300)
m.turn_right(int(c.RIGHT_TURN_TIME / 2))
s.backwards_until_black_lfcliff()
s.align_far_fcliffs()
msleep(300)
m.backwards(600)
msleep(300)
ao()
# DON'T DELETE THESE NEXT 4 LINES. They are purposeful. It avoids the roomba going into sleep mode after the calibration and not starting right.
create_disconnect()
wait_for_light(c.LIGHT_SENSOR)
create_connect()
shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION