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 bumpfollow_right_until_black_right(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.isRightOnWhite():
base_bumpfollow_right()
if should_stop:
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()