def backwards_gyro_until_both_white_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 s.isRightOnBlack():
while seconds() < sec 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.
else:
while seconds() < sec 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 determine_gyro_conversion_rate():
angle = 0
print "Starting determine_gyro_conversion_rate()"
print "Starting s.isLeftOnWhite()"
while s.isLeftOnWhite():
msleep(10)
angle += (get_change_in_angle() - bias) * 10
print "Starting s.isLeftOnBlack()"
while s.isLeftOnBlack():
msleep(10)
angle += (get_change_in_angle() - bias) * 10
print "Starting s.isLeftOnWhite()"
while s.isLeftOnWhite():
msleep(10)
angle += (get_change_in_angle() - bias) * 10
print "Starting s.isLeftOnBlack()"
while s.isLeftOnBlack():
msleep(10)
angle += (get_change_in_angle() - bias) * 10
print "Starting s.isLeftOnWhite()"
while s.isLeftOnWhite():
msleep(10)
angle += (get_change_in_angle() - bias) * 10
print "Deactivating motors"
m.deactivate_motors()
print "Finished deactivating motors"
c.DEGREE_CONVERSION_RATE = abs(angle / 360.0) #- 92
print "DEGREE_CONVERSION_RATE: " + str(c.DEGREE_CONVERSION_RATE)
def bumpfollow_right_until_black_left(time=c.SAFETY_TIME, should_stop=True):
if time == 0:
should_stop = False
time = c.SAFETY_TIME
sec = seconds() + time / 1000.0
while seconds() < sec and s.isLeftOnWhite():
base_bumpfollow_right()
if should_stop:
deactivate_motors()