Protobuf prototype

client_s1/docs/source/lib/Linefollower.rst

@@ -0,0 +1,79 @@
+.. _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()