.. _lib_Linefollower:

Linefollower Examples
*********************

Simple Linefollower
-------------------------

.. code-block:: python

    from compLib.Motor import Motor
    from compLib.Display import Display
    from compLib.IRSensor import IRSensor
    from compLib.Encoder import Encoder

    import time

    IRSensor.set(1, True)
    IRSensor.set(2, True)
    IRSensor.set(3, True)
    IRSensor.set(4, True)
    IRSensor.set(5, True)

    DRIVE_SPEED = 75
    COLOR_BREAK = 900
    KP = 10.0
    KD = 0.0

    def drive(leftSpeed, rightSpeed):
        rightSpeed *= -0.906

        Motor.power(1, min(max(-100, rightSpeed), 100))
        Motor.power(4, min(max(-100, leftSpeed), 100))

    def follow(sleepTime = 0.1):
        lastError = 0
        sensorsBlack = 0
        while sensorsBlack < 3:
            sensorsBlack = 0
            for i in range(1, 6):
                if IRSensor.read(i) > COLOR_BREAK:
                    sensorsBlack += 1

            error = lastError
            if IRSensor.read(3) > COLOR_BREAK:
                error = 0
            elif IRSensor.read(1) > COLOR_BREAK:
                error = -1.5
            elif IRSensor.read(5) > COLOR_BREAK:
                error = 1.5
            elif IRSensor.read(2) > COLOR_BREAK:
                error = -1
            elif IRSensor.read(4) > COLOR_BREAK:
                error = 1
            elif error == 1.5:
                error = 3
            elif error == -1.5:
                error = -3

            lastError = error

            adjustment = KP * error + KD * (error - lastError)
            leftSpeed = DRIVE_SPEED + adjustment
            rightSpeed = DRIVE_SPEED - adjustment

            print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
            drive(leftSpeed, rightSpeed)

        drive(0, 0)
        time.sleep(sleepTime)

    def main():
        follow()
        follow()
        follow()
        follow()
        follow(0.2)

    main()