def calibrate_motors():
print("Running calibrate()")
# This initializes all variables to 0.
angle, error, i, total_left_speed, total_right_speed = (0, ) * 5
# Robot goes for a certain distance, based on the motors' "tics."
left_motor.clear_tics()
right_motor.clear_tics()
m.activate_motors(int(-left_motor.base_power / 2),
int(-right_motor.base_power / 2))
while abs(left_motor.get_tics() + right_motor.get_tics()) / 2 < 1000:
KIPR.msleep(10)
angle += (s.get_change_in_angle() - gyro_bias) * 10
error = 0.034470956 * angle # Positive error means veering left. Negative means veering right.
left_speed = (-left_motor.base_power + error) / 2
right_speed = (-right_motor.base_power + error) / 2
total_left_speed += left_speed
total_right_speed += right_speed
i += 1
m.activate_motors(left_speed, right_speed)
m.deactivate_motors()
# All motor power variables are set based on the gyro drive in calibration.
avg_left_speed = total_left_speed / i
avg_right_speed = total_right_speed / i
left_motor.set_all_powers(-avg_left_speed * 2)
right_motor.set_all_powers(-avg_right_speed * 2)
print("Finished Calibrating. Moving back into starting box...\n")
def backwards_gyro(time):
angle = 0
error = 0
sec = seconds() + time / 1000.0
while seconds() < sec:
left_speed = -c.BASE_LM_POWER + error
right_speed = -c.BASE_RM_POWER - error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - bias) * 10
error = 0.003830106222 * angle # Positive error means veering right. Negative means veering left.
m.deactivate_motors()
def wfollow_left(time, refresh_rate=c.LFOLLOW_REFRESH_RATE):
print "Starting wfollow_left()"
sec = seconds() + time / 1000.0
while seconds() < sec:
if BumpedLeft() or BumpedLightLeft() or BumpedLightFrontLeft():
m.activate_motors(c.BASE_LM_POWER,
int(c.LFOLLOW_SMOOTH_RM_POWER * 0.6))
else:
m.activate_motors(int(c.LFOLLOW_SMOOTH_LM_POWER * 0.5),
c.BASE_RM_POWER)
msleep(refresh_rate)
m.deactivate_motors()
def calibrate():
# Code to calibrate the bw values. This goes before every run. Ends with light sensor calibration.
max_sensor_value_right = 0
min_sensor_value_right = 90000
max_sensor_value_left = 0
min_sensor_value_left = 90000
max_sensor_value_third = 0
min_sensor_value_third = 90000
cmpc(c.LEFT_MOTOR)
cmpc(c.RIGHT_MOTOR)
if c.IS_MAIN_BOT:
calibrate_tics = 2000
else: # Clone bot
calibrate_tics = 2500
print "Running calibrate()"
m.activate_motors(int (c.BASE_LM_POWER / 3), int(c.BASE_RM_POWER / 3))
while gmpc(c.LEFT_MOTOR) < calibrate_tics and gmpc(c.RIGHT_MOTOR) < calibrate_tics:
if analog(c.RIGHT_TOPHAT) > max_sensor_value_right:
max_sensor_value_right = analog(c.RIGHT_TOPHAT)
if analog(c.RIGHT_TOPHAT) < min_sensor_value_right:
min_sensor_value_right = analog(c.RIGHT_TOPHAT)
if analog(c.LEFT_TOPHAT) > max_sensor_value_left:
max_sensor_value_left = analog(c.LEFT_TOPHAT)
if analog(c.LEFT_TOPHAT) < min_sensor_value_left:
min_sensor_value_left = analog(c.LEFT_TOPHAT)
if analog(c.THIRD_TOPHAT) > max_sensor_value_third:
max_sensor_value_third = analog(c.THIRD_TOPHAT)
if analog(c.THIRD_TOPHAT) < min_sensor_value_third:
min_sensor_value_third = analog(c.THIRD_TOPHAT)
msleep(1)
m.deactivate_motors()
c.LEFT_TOPHAT_BW = int(((max_sensor_value_left + min_sensor_value_left) / 2)) - 1000
c.RIGHT_TOPHAT_BW = int(((max_sensor_value_right + min_sensor_value_right) / 2)) - 1000
if c.IS_MAIN_BOT:
c.THIRD_TOPHAT_BW = int(((max_sensor_value_third + min_sensor_value_third) / 2))
else: # Clone bot
c.THIRD_TOPHAT_BW = int(((max_sensor_value_third + min_sensor_value_third) / 2)) + 300
print "max_sensor_value_left: " + str(max_sensor_value_left)
print "min_sensor_value_left: " + str(min_sensor_value_left)
print "Target Left " + str(c.LEFT_TOPHAT_BW)
print "max_sensor_value_right: " + str(max_sensor_value_right)
print "min_sensor_value_right: " + str(min_sensor_value_right)
print "Target Right " + str(c.RIGHT_TOPHAT_BW)
print "max_sensor_value_third: " + str(max_sensor_value_third)
print "min_sensor_value_third: " + str(min_sensor_value_third)
print "Target Third " + str(c.THIRD_TOPHAT_BW)
f.backwards_through_two_lines_in_calibration()
f.align_close()
m.backwards(600)
msleep(25)
ao()
msleep(1000)
def wfollow_right_until_white_right(time=15000,
refresh_rate=c.LFOLLOW_REFRESH_RATE):
print "Starting wfollow_right_until_white_right()"
sec = seconds() + time / 1000.0
while seconds() < sec and BlackRight():
if BumpedRight() or BumpedLightRight() or BumpedLightFrontRight():
m.activate_motors(int(c.LFOLLOW_SMOOTH_RM_POWER * 0.6),
c.BASE_RM_POWER)
else:
m.activate_motors(c.BASE_LM_POWER,
int(c.LFOLLOW_SMOOTH_RM_POWER * 0.5))
msleep(refresh_rate)
m.deactivate_motors()
def forwards_gyro(time, should_stop=True):
angle = 0
error = 0
sec = seconds() + time / 1000.0
while seconds() < sec:
left_speed = c.BASE_LM_POWER + error
right_speed = c.BASE_RM_POWER - error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - u.gyro_bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
if should_stop:
m.deactivate_motors()
def backwards_gyro_until_pressed_bump_switch(time=c.SAFETY_TIME):
angle = 0
error = 0
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec and s.isBumpSwitchNotPressed():
left_speed = -c.BASE_LM_POWER + error
right_speed = -c.BASE_RM_POWER - error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()
def forwards_gyro_until_black_cliffs(time=c.SAFETY_TIME):
angle = 0
error = 0
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec and s.isRightOnWhite() and s.isLeftOnWhite():
left_speed = c.BASE_LM_POWER + error
right_speed = c.BASE_RM_POWER - error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()
def backwards_gyro_until_item_is_in_claw(time=c.SAFETY_TIME):
angle = 0
error = 0
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec and s.isNothingInClaw():
left_speed = -c.BASE_LM_POWER + error
right_speed = -c.BASE_RM_POWER - error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - u.gyro_bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()
def lfollow_until(self,
boolean_function,
mode=c.STANDARD,
bias=10,
*,
should_stop=True,
time=c.SAFETY_TIME):
target = 100.0 * (self.value_midpoint - self.get_black_value()) / (
self.get_white_value() - self.get_black_value()) + bias
last_error = 0
integral = 0
if time == 0:
should_stop = False
sec = seconds() + time / 1000
while seconds() < sec and not (boolean_function()):
norm_reading = 100.0 * (self.get_value() - self.get_black_value()
) / (self.get_white_value() -
self.get_black_value())
error = target - norm_reading # Positive error means black, negative means white.
derivative = error - last_error # If rate of change is going negative, need to veer left
last_error = error
integral = 0.5 * integral + error
if error > 25 or error < -25:
kp = 3.2
ki = c.KI
kd = c.KD
elif error < 10 and error > -10:
kp = 0.1
ki = c.KI / 1000
kd = c.KD / 1000
else:
kp = 0.35
ki = c.KI / 100
kd = c.KD / 100
left_power = c.BASE_LM_POWER + (
(kp * error) + (ki * integral) +
(kd * derivative)) * self.side * mode
right_power = c.BASE_RM_POWER - (
(kp * error) + (ki * integral) +
(kd * derivative)) * self.side * mode
m.activate_motors(int(left_power), int(right_power))
c.CURRENT_LM_POWER = left_power
c.CURRENT_RM_POWER = right_power
msleep(1)
if should_stop:
m.deactivate_motors()
def backwards_gyro(time, should_stop=True):
angle = 0
error = 0
memory = 0
if time == 0:
should_stop = False
time = c.SAFETY_TIME
sec = seconds().time + time / 1000.0
while seconds().time < sec:
left_speed = -left_motor.base_power + (error + memory)
right_speed = -right_motor.base_power - (error + memory)
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (get_change_in_angle() - u.gyro_bias) * 10
error = 0.034470956 * angle # Positive error means veering right. Negative means veering left.
memory += 0.001 * error
if should_stop:
m.deactivate_motors()
def backwards_gyro(time, should_stop=True):
angle = 0
error = 0
memory = 0
if time == 0:
should_stop = False
time = c.SAFETY_TIME
sec = seconds() + time / 1000.0
while seconds() < sec:
left_speed = -c.BASE_LM_POWER + error + memory
right_speed = -c.BASE_RM_POWER + error + memory
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (get_change_in_angle() - bias) * 10
error = 0.034470956 * angle # Positive error means veering right. Negative means veering left.
memory += 0.001 * error
if should_stop:
m.deactivate_motors()
def drive_gyro_until(boolean_function, time=c.SAFETY_TIME, should_stop=True):
angle = 0
error = 0
memory = 0
if time == 0:
should_stop = False
time = c.SAFETY_TIME
sec = seconds() + time / 1000.0
while seconds() < sec and not (boolean_function()):
left_speed = left_motor.base_power + (error + memory)
right_speed = right_motor.base_power - (error + memory)
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (get_change_in_angle() - u.gyro_bias) * 10
error = 0.034470956 * angle # Positive error means veering left. Negative means veering right.
memory += 0.00001 * error
if should_stop:
m.deactivate_motors()
def drive_gyro_until_black_right_or_fourth(time=c.SAFETY_TIME,
should_stop=True):
angle = 0
error = 0
memory = 0
if time == 0:
should_stop = False
time = c.SAFETY_TIME
sec = seconds() + time / 1000.0
while seconds() < sec and s.isFourthOnWhite() and s.isRightOnWhite():
left_speed = c.BASE_LM_POWER + error
right_speed = c.BASE_RM_POWER + error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (get_change_in_angle() - bias) * 10
error = 0.034470956 * angle # Positive error means veering left.
memory += 0.001 * error
if should_stop:
m.deactivate_motors()
def forwards_gyro_wall_assisted_on_left(time=c.SAFETY_TIME, kp=1):
angle = 0
error = 0
target_angle = 0
first_bump = False
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec:
if s.isRoombaBumped():
if first_bump == True:
time_at_last_check = seconds()
time_on_wall = 0
time_on_wall += (seconds() - time_at_last_check)
time_at_last_check = seconds()
u.halve_speeds()
if s.isLeftBumped() and not (
s.isRightBumped()) and time_on_wall < 0.1:
print "Left is bumped, early on the wall."
target_angle -= 10
print "(Left) Target angle: " + str(target_angle)
elif s.isLeftBumped() and not (
s.isRightBumped()) and time_on_wall >= 0.1:
print "Left is bumped, late on wall."
target_angle -= 2
u.normalize_speeds()
print "(Left) Target angle: " + str(target_angle)
else:
print "Both bumped. Turning away faster."
target_angle -= 70
print "(Left) Target angle: " + str(target_angle)
first_bump = False
else:
first_bump = True
u.normalize_speeds()
left_speed = c.BASE_LM_POWER - error
right_speed = c.BASE_RM_POWER + error
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - bias) * 10
error = target_angle - 0.003830106222 * kp * angle # Negative error means veering left. Positive means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()
def calibrate_tophats_and_motors(big_tophat_bias=-1000, small_tophat_bias=600):
# Code to calibrate the tophat midpoint values and motor speeds that go straight.
# If sensing black when it should be sensing white, increase bias
# If sensing white when it should be sensing black, decrease bias
print("Running calibrate()")
angle = 0
error = 0
i = 0
total_left_speed = 0
total_right_speed = 0
# Robot goes for a certain distance, based on the motors' "tics."
left_motor.clear_tics()
right_motor.clear_tics()
m.activate_motors(int(-left_motor.base_power / 2),
int(-right_motor.base_power / 2))
while abs(left_motor.get_tics() +
right_motors.get_tics()) / 2 < calibrate_tics:
left_speed = (-left_motor.base_power + error) / 2
right_speed = (-right_motor.base_power + error) / 2
total_left_speed += left_speed
total_right_speed += right_speed
i += 1
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (s.get_change_in_angle() - gyro_bias) * 10
error = 0.034470956 * angle # Positive error means veering left. Negative means veering right.
# For all tophats, the sensor finds the maximum and minimum values it goes over during the calibration sequence.
for tophat in Tophat.all_tophats:
tophat.compare_and_replace_extremes()
m.deactivate_motors()
for tophat in Tophat.all_tophats:
if tophat.tophat_type == c.BIG_TOPHAT:
tophat.determine_midpoint_from_extremes(big_tophat_bias)
else:
tophat.determine_midpoint_from_extremes(small_tophat_bias)
# All motor power variables are set based on the gyro drive in calibration.
avg_left_speed = total_left_speed / i
avg_right_speed = total_right_speed / i
left_motor.set_all_powers(-avg_left_speed * 2)
right_motor.set_all_powers(-avg_right_speed * 2)
print("Finished Calibrating. Moving back into starting box...\n")
def test_gyro(time=20000):
print "Starting test_gyro"
theta = 0
m.activate_motors(c.BASE_LM_POWER, c.BASE_RM_POWER * -1)
sec = seconds() + time / 1000.0
turn_time = seconds() + c.RIGHT_TURN_TIME / 1000.0
while seconds() < sec:
sec_2 = seconds() + 1
while seconds() < sec_2:
msleep(10)
if seconds() > turn_time:
print "Turn time reached"
m.deactivate_motors()
print str(theta)
print str(gyro_x() - c.AVG_BIAS)
msleep(300)
if c.CURRENT_LM_POWER == 0 and c.CURRENT_RM_POWER == 0:
pass
else:
theta += (gyro_x() - c.AVG_BIAS)
print str(theta)
print str(gyro_x() - c.AVG_BIAS)
def calibrate_tophats(big_tophat_bias=-1000, small_tophat_bias=600):
"""Code to calibrate the tophat midpoint values."""
# If sensing black when it should be sensing white, increase bias.
# If sensing white when it should be sensing black, decrease bias.
print("Running calibrate()")
# Robot goes for a certain distance, based on the motors' "tics."
left_motor.clear_tics()
right_motor.clear_tics()
m.activate_motors(int(-left_motor.base_power / 2),
int(-right_motor.base_power / 2))
while abs(left_motor.get_tics() +
right_motor.get_tics()) / 2 < calibrate_tics:
for tophat in Tophat.all_tophats:
tophat.compare_and_replace_extremes()
KIPR.msleep(1)
m.deactivate_motors()
for tophat in Tophat.all_tophats:
if tophat.tophat_type == c.BIG_TOPHAT:
tophat.determine_midpoint_from_extremes(big_tophat_bias)
else:
tophat.determine_midpoint_from_extremes(small_tophat_bias)
print("Finished Calibrating. Moving back into starting box...\n")
def drive_gyro(time, kp=10, ki=.1, kd=1, stop=True):
error = 0
last_error = 0
integral = 0
derivative = 0
first_run_through = True
sec = seconds() + time / 1000.0
while seconds() < sec:
error = (gyro_x() - c.AVG_BIAS) * c.WALLAGREES_TO_DEGREES_RATE
derivative = error - last_error # If rate of change is going negative, need to veer left
last_error = error
integral = 0.5 * integral + error
c.ROBOT_ANGLE += error
left_power = c.BASE_LM_POWER + ((kp * error) + (ki * integral) +
(kd * derivative))
right_power = c.BASE_RM_POWER + (
(kp * error) + (ki * integral) +
(kd * derivative)) # Addition decreases power here
if left_power > 1300:
left_power = 1300
elif left_power < -1000:
left_power = -1000
if right_power < -1300:
right_power = -1300
elif right_power > 1000:
right_power = 1000
if first_run_through == True:
m.activate_motors(
int(left_power), int(right_power)
) # this is where the accelerate command would go if we had one.
else:
m.activate_motors(int(left_power), int(right_power))
first_run_through = False
msleep(c.GYRO_TIME)
if stop == True:
m.deactivate_motors()
def forwards_gyro_wall_assisted_on_right(time=c.SAFETY_TIME, kp=1):
angle = 0
error = 0
target_angle = 0
if time == 0:
time = c.SAFETY_TIME_NO_STOP
sec = seconds() + time / 1000.0
while seconds() < sec:
if s.isRoombaBumped():
u.halve_speeds()
kp = 5
target_angle -= 0.1
print "(Right) Target angle: " + str(target_angle)
else:
u.normalize_speeds()
kp = 1
left_speed = c.BASE_LM_POWER - error
right_speed = c.BASE_RM_POWER + error
m.activate_motors(left_speed, right_speed)
msleep(100)
angle += (gyro_z() - bias) * 100
error = target_angle - 0.003830106222 * kp * angle # Negative error means veering left. Positive means veering right.
if time != c.SAFETY_TIME_NO_STOP:
m.deactivate_motors()
def calibrate_motor_powers():
angle = 0
error = 0
i = 0
total_left_speed = 0
total_right_speed = 0
sec = seconds() + 3
while seconds() < sec:
left_speed = c.BASE_LM_POWER + error
right_speed = c.BASE_RM_POWER - error
total_left_speed += left_speed
total_right_speed += right_speed
i += 1
m.activate_motors(left_speed, right_speed)
msleep(10)
angle += (gyro_z() - bias) * 10
error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right.
m.deactivate_motors()
avg_left_speed = total_left_speed / i
avg_right_speed = total_right_speed / i
c.BASE_LM_POWER = int(avg_left_speed)
c.BASE_RM_POWER = int(avg_right_speed)
print "c.BASE_LM_POWER: " + str(c.BASE_LM_POWER)
print "c.BASE_RM_POWER: " + str(c.BASE_RM_POWER)
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
sec = seconds() + 9
print "Running calibrate()"
m.activate_motors(-1, 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) + 750
c.RCLIFF_BW = (
(max_sensor_value_rcliff + min_sensor_value_rcliff) / 2) + 750
c.LFCLIFF_BW = (
(max_sensor_value_lfcliff + min_sensor_value_lfcliff) / 2) + 770
c.RFCLIFF_BW = (
(max_sensor_value_rfcliff + min_sensor_value_rfcliff) / 2) + 1000
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)
f.forwards_until_black_lfcliff()
f.align_close_fcliffs()
msleep(300)
f.forwards_until_black_lcliff()
f.align_close_cliffs()
msleep(300)
f.forwards_until_bump()
m.backwards(100)
#m.activate_motors(1)
#msleep(4250)
#m.deactivate_motors()
msleep(1500)
ao()
create_disconnect()
wait_for_light(c.LIGHT_SENSOR)
create_connect()
shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
def forwards_until_depth_senses_object():
m.activate_motors(1)
while NotDepthSensesObject():
pass
m.deactivate_motors()
def forwards_until_bump():
print "Starting forwards_until_bump()"
m.activate_motors(1)
while leftIsNotBumped() and rightIsNotBumped():
pass
m.deactivate_motors()
def left_point_turn_until_lfcliff_senses_black():
m.activate_motors(1, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER)
while NotBlackFrontLeft():
pass
m.deactivate_motors()
def backwards_until_white_lcliff():
print "Start drive_until_black_lcliff"
m.activate_motors()
while BlackLeft():
pass
m.deactivate_motors()
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 test_veer(time=10000):
m.activate_motors()
msleep(time)
m.deactivate_motors()
sd()
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 forwards_until_black_lcliff():
print "Start drive_until_black_lcliff"
m.activate_motors(1)
while NotBlackLeft():
pass
m.deactivate_motors()
