def straight_line_light_black(angle, speed, color_sensor):
drive_base.reset()
while color_sensor.reflection() <= 10:
fix_amount = p_controller(angle, gyro)
drive_base.drive(speed, -fix_amount)
drive_base.stop()
def straight_line(angle, speed, duration):
timer = StopWatch()
while timer.time() <= duration:
screen.clear()
fix_amount = p_controller(angle, gyro)
drive_base.drive(speed, -fix_amount)
def follow_line_left(speed, duration):
while timer.time() < duration:
deviation = color_sensor.reflection() - setpoint
turn_rate = K_P * deviation
drive_base.drive(speed, turn_rate)
wait(10)
def straight_line_distance(angle, speed, distance):
drive_base.reset()
while abs(drive_base.distance()) <= distance:
fix_amount = p_controller(angle, gyro)
screen.print(gyro.angle())
drive_base.drive(speed, -fix_amount)
drive_base.stop()
def follow_line_right(speed, duration):
timer = StopWatch()
while timer.time() <= duration:
deviation = right_color_sensor.reflection() - setpoint
turn_rate = K_P * deviation
screen.print(right_color_sensor.reflection())
drive_base.drive(speed, turn_rate)
wait(10)
def follow_line_distance_left(speed, distance):
timer = StopWatch()
drive_base.reset()
while abs(drive_base.distance()) <= distance:
deviation = left_color_sensor.reflection() - setpoint
turn_rate = K_P * deviation
screen.print(left_color_sensor.reflection())
drive_base.drive(speed, turn_rate)
wait(10)
def turn_white(angle, threshold, color_sensor):
while color_sensor.reflection() >= 60:
fix_amount = p_controller(angle, gyro)
drive_base.drive(0, -fix_amount)
def turn(angle, threshold):
while gyro.angle() <= angle - threshold or gyro.angle(
) >= angle + threshold:
fix_amount = p_controller(angle, gyro)
drive_base.drive(0, -fix_amount)
