Linefollower Examples

Simple Linefollower

import time

from compLib.IRWrapper import IRWrapper
from compLib.Motor import Motor

STRAIGHT_SPEED = 40.0
TURN_SPEED = 40.0

COLOR_BREAK = 500


def drive(left, right, active_break=False, back_mult=0.0):
    left = max(min(left, 100), -100)
    right = max(min(right, 100), -100)

    print("left {} right {}".format(left, right))

    Motor.power(0, left)
    Motor.power(1, left * back_mult)
    Motor.power(2, right * back_mult)
    Motor.power(3, right)

    if active_break:
        time.sleep(0.001)

        Motor.power(0, 10)
        Motor.power(1, 10)
        Motor.power(2, 10)
        Motor.power(3, 10)

        time.sleep(0.001)


def follow_till_line():
    while True:
        print("left: {:.2f}  middle: {:.2f}  right: {:.2f}".format(IRWrapper.bottom_left_calibrated(),
                                                                   IRWrapper.bottom_middle_calibrated(),
                                                                   IRWrapper.bottom_right_calibrated()))

        left = IRWrapper.bottom_left_calibrated()
        right = IRWrapper.bottom_right_calibrated()

        if left > COLOR_BREAK:
            drive(-TURN_SPEED, TURN_SPEED, back_mult=1.25)
            while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
                    IRWrapper.bottom_right_calibrated() < COLOR_BREAK:
                pass
            drive(0, 0)
        elif right > COLOR_BREAK:
            drive(TURN_SPEED, -TURN_SPEED, back_mult=1.25)
            while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
                    IRWrapper.bottom_left_calibrated() < COLOR_BREAK:
                pass
            drive(0, 0)
        else:
            drive(STRAIGHT_SPEED, STRAIGHT_SPEED, True)

        if IRWrapper.bottom_left_calibrated() > 750 and IRWrapper.bottom_right_calibrated() > 750:
            break

    print("left: {}  middle: {}  right: {}".format(IRWrapper.bottom_left_calibrated(),
                                                   IRWrapper.bottom_middle_calibrated(),
                                                   IRWrapper.bottom_right_calibrated()))

    drive(0, 0)

    time.sleep(0.2)


if __name__ == "__main__":
    # Put bot with middle sensor onto black line
    IRWrapper.calibrate()

    # Initialize Motors, so the bot doesn't speed off instantly
    drive(1, 1)
    time.sleep(1)

    # Follow line, till the left and right sensor are on black
    follow_till_line()