From 1c0310b9c78fffe7a7bf6b5d1cec740228d1c8f9 Mon Sep 17 00:00:00 2001
From: Konstantin Lampalzer <konstantin@localhost.systems>
Date: Tue, 14 Sep 2021 18:34:50 +0200
Subject: [PATCH] new linefollower example

---
 docs/source/lib/Linefollower.rst | 124 +++++++++++++++----------------
 1 file changed, 58 insertions(+), 66 deletions(-)

diff --git a/docs/source/lib/Linefollower.rst b/docs/source/lib/Linefollower.rst
index 8918554..48fe4d3 100644
--- a/docs/source/lib/Linefollower.rst
+++ b/docs/source/lib/Linefollower.rst
@@ -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()
\ No newline at end of file