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():
# 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
total_tophat_reading = 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()
total_tophat_reading += analog(c.CLAW_TOPHAT)
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()
# If sensing black when should be sensing white, decrease bias.
# If sensing white when should be sensing black, increase bias
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) + 100
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)
c.CLAW_TOPHAT_NOTHING_READING = total_tophat_reading / run_throughs
#print "\nPut coupler in claw and press the right button..."
#s.wait_until(isRightButtonPressed)
#c.CLAW_TOPHAT_COUPLER_READING = analog(c.CLAW_TOPHAT)
#c.CLAW_TOPHAT_BW = (c.CLAW_TOPHAT_COUPLER_READING + c.CLAW_TOPHAT_NOTHING_READING) / 2
#print "c.CLAW_TOPHAT_BW: " + str(c.CLAW_TOPHAT_BW)
#if c.CLAW_TOPHAT_COUPLER_READING > c.CLAW_TOPHAT_NOTHING_READING:
# print "Coupler reading is higher."
#else:
# print "Nothing reading is higher."
s.backwards_until_black_cliffs()
s.align_far_cliffs()
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()
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