compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

new linefollower example

Browse source
This commit is contained in:
Konstantin Lampalzer 2021-09-14 18:34:50 +02:00
parent 3ef0fff256
commit 1c0310b9c7
1 changed files with 58 additions and 66 deletions
Download patch file Download diff file Expand all files Collapse all files

124
docs/source/lib/Linefollower.rst
View file

@ -8,82 +8,74 @@ 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
from compLib.IRWrapper import IRWrapper
from compLib.Motor import Motor
IRSensor.set(1, True)
IRSensor.set(2, True)
IRSensor.set(3, True)
IRSensor.set(4, True)
IRSensor.set(5, True)
STRAIGHT_SPEED = 40.0
TURN_SPEED = 40.0
DRIVE_SPEED = 85
TURN_SPEED = 50
COLOR_BREAK = 750
KP = 50.0
KD = 0.0
COLOR_BREAK = 500
def drive(leftSpeed, rightSpeed):
leftSpeed *= -1
rightSpeed *= 0.906
Motor.power(1, min(max(-100, leftSpeed), 100))
Motor.power(2, min(max(-100, rightSpeed), 100))
def drive(left, right, active_break=False, back_mult=0.0):
left = max(min(left, 100), -100)
right = max(min(right, 100), -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
print("left {} right {}".format(left, right))
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
Motor.power(0, left)
Motor.power(1, left * back_mult)
Motor.power(2, right * back_mult)
Motor.power(3, right)
adjustment = KP * error + KD * (error - lastError)
leftSpeed = DRIVE_SPEED + adjustment
rightSpeed = DRIVE_SPEED - adjustment
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()))
print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
drive(leftSpeed, rightSpeed)
drive(0, 0)
time.sleep(sleepTime)
time.sleep(0.2)
def main():
follow()
follow()
follow()
follow()
follow(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()
main()