Update

client_s2/lf.py

@@ -0,0 +1,109 @@
+import time
+
+from compLib.CompLibClient import CompLibClient
+from compLib.IRSensor import IRSensor
+from compLib.Motor import Motor
+
+DRIVE_SPEED = 5.0
+COLOR_BREAK = 1500.0
+KP = 2.0
+KD = 0.0
+
+
+def drive(left_speed, right_speed):
+    print(left_speed, right_speed)
+
+    right_speed *= -1.0
+
+    Motor.speed(0, right_speed)
+    Motor.speed(3, left_speed)
+
+
+def follow(sleep_time=0.1):
+    last_error = 0
+    sensors_black = 0
+    while sensors_black <= 3:
+        sensor_values = IRSensor.read_all()
+
+        sensors_black = 0
+        for sensor in sensor_values:
+            if sensor > COLOR_BREAK:
+                sensors_black += 1
+
+        error = last_error
+        if sensor_values[2] > COLOR_BREAK:
+            error = 0
+        elif sensor_values[0] > COLOR_BREAK:
+            error = -1.5
+        elif sensor_values[4] > COLOR_BREAK:
+            error = 1.5
+        elif sensor_values[1] > COLOR_BREAK:
+            error = -1
+        elif sensor_values[3] > COLOR_BREAK:
+            error = 1
+        elif error == 1.5:
+            error = 3.5
+        elif error == -1.5:
+            error = -3.5
+
+        last_error = error
+
+        adjustment = KP * error + KD * (error - last_error)
+        left_speed = DRIVE_SPEED + adjustment
+        right_speed = DRIVE_SPEED - adjustment
+
+        print(sensor_values)
+        print(f"{left_speed} {right_speed} {adjustment} {error}")
+        drive(left_speed, right_speed)
+
+    drive(0, 0)
+    time.sleep(sleep_time)
+
+
+def follow_simple():
+    left_speed = DRIVE_SPEED
+    right_speed = DRIVE_SPEED
+    sensor_values = IRSensor.read_all()
+    while True:
+        sensor_values = IRSensor.read_all()
+        # for i in range(len(sensor_values)):
+        #     sensor_values[i] = (sensor_values[i] + new_sensor_values[i]) / 2.0
+
+        print(sensor_values)
+
+        if sensor_values[0] > COLOR_BREAK and sensor_values[4] > COLOR_BREAK:
+            break
+
+        if sensor_values[0] > COLOR_BREAK:
+            left_speed = -DRIVE_SPEED / 2
+            right_speed = DRIVE_SPEED
+        elif sensor_values[4] > COLOR_BREAK:
+            left_speed = DRIVE_SPEED
+            right_speed = -DRIVE_SPEED / 2
+        elif sensor_values[2] > COLOR_BREAK:
+            left_speed = DRIVE_SPEED
+            right_speed = DRIVE_SPEED
+
+        drive(left_speed, right_speed)
+
+
+def main():
+    CompLibClient.use_unix_socket()
+
+    IRSensor.enable()
+    time.sleep(0.1)
+
+    # while True:
+    #     print(IRSensor.read_all())
+
+    # follow_simple()
+    # drive(5, 5)
+    # time.sleep(5)
+    # follow()
+    # follow()
+    # follow()
+    # follow()
+    # follow(0.2)
+
+
+main()