Linefollower Examples

Simple Linefollower

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()