89 lines
2.8 KiB
ReStructuredText
89 lines
2.8 KiB
ReStructuredText
.. _lib_Linefollower:
|
|
|
|
Linefollower Examples
|
|
*********************
|
|
|
|
Simple Linefollower
|
|
-------------------------
|
|
|
|
.. code-block:: python
|
|
|
|
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()
|