79 lines
No EOL
1.9 KiB
ReStructuredText
79 lines
No EOL
1.9 KiB
ReStructuredText
.. _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() |