update documentation

client/lf.py

@@ -1,109 +1,96 @@
-import time
-
-from compLib.CompLibClient import CompLibClient
-from compLib.IRSensor import IRSensor
 from compLib.Motor import Motor
+from compLib.IRSensor import IRSensor
+from compLib.CompLibClient import CompLibClient
 
-DRIVE_SPEED = 5.0
-COLOR_BREAK = 1500.0
-KP = 2.0
+import time
+import math
+
+DRIVE_SPEED = 20.0
+COLOR_BREAK = 850
+KP = 7.5
 KD = 0.0
 
+SAMPLE_TIME_S = 0.001
+CUTOFF_FREQ_HZ = 50.0
 
-def drive(left_speed, right_speed):
-    print(left_speed, right_speed)
+RC = 1.0 / (2.0 * math.pi * CUTOFF_FREQ_HZ)
 
-    right_speed *= -1.0
+FIRST_COEFF = SAMPLE_TIME_S / (SAMPLE_TIME_S + RC)
+SECOND_COEFF = RC / (SAMPLE_TIME_S + RC)
 
-    Motor.speed(0, right_speed)
-    Motor.speed(3, left_speed)
+out_old = 0.0
+start_time = time.time()
 
 
-def follow(sleep_time=0.1):
-    last_error = 0
-    sensors_black = 0
-    while sensors_black <= 3:
-        sensor_values = IRSensor.read_all()
+def drive(leftSpeed, rightSpeed):
+    rightSpeed *= -0.906
 
-        sensors_black = 0
-        for sensor in sensor_values:
-            if sensor > COLOR_BREAK:
-                sensors_black += 1
+    Motor.multiple_power((0, min(max(-100, rightSpeed), 100)), (3, min(max(-100, leftSpeed), 100)))
 
-        error = last_error
-        if sensor_values[2] > COLOR_BREAK:
+def follow(sleepTime = 0.3):
+    global out_old
+    lastError = 0
+    sensorsBlack = 0
+    while sensorsBlack < 3:
+        sensor_data = IRSensor.read_all()
+
+        sensorsBlack = 0
+        for i in range(0, 5):
+            if sensor_data[i] > COLOR_BREAK:
+                sensorsBlack += 1
+
+
+        middle_sensor = sensor_data[2]
+        filtered_sensor = FIRST_COEFF * middle_sensor + SECOND_COEFF * out_old
+        out_old = filtered_sensor
+
+        sample_time = str(time.time() - start_time).replace(".", ",")
+        print(f"{sample_time} {middle_sensor} {int(filtered_sensor)}")
+
+        error = lastError
+        if sensor_data[2] > COLOR_BREAK:
             error = 0
-        elif sensor_values[0] > COLOR_BREAK:
+        elif sensor_data[0] > COLOR_BREAK:
             error = -1.5
-        elif sensor_values[4] > COLOR_BREAK:
+        elif sensor_data[4] > COLOR_BREAK:
             error = 1.5
-        elif sensor_values[1] > COLOR_BREAK:
+        elif sensor_data[1] > COLOR_BREAK:
             error = -1
-        elif sensor_values[3] > COLOR_BREAK:
+        elif sensor_data[3] > COLOR_BREAK:
             error = 1
         elif error == 1.5:
-            error = 3.5
+            error = 3
         elif error == -1.5:
-            error = -3.5
+            error = -3
 
-        last_error = error
+        lastError = error
 
-        adjustment = KP * error + KD * (error - last_error)
-        left_speed = DRIVE_SPEED + adjustment
-        right_speed = DRIVE_SPEED - adjustment
+        adjustment = KP * error + KD * (error - lastError)
+        leftSpeed = DRIVE_SPEED + adjustment
+        rightSpeed = DRIVE_SPEED - adjustment
 
-        print(sensor_values)
-        print(f"{left_speed} {right_speed} {adjustment} {error}")
-        drive(left_speed, right_speed)
+        # print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
+        drive(leftSpeed, rightSpeed)
+
+        time.sleep(SAMPLE_TIME_S)
 
     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)
-
+    time.sleep(sleepTime)
 
 def main():
     CompLibClient.use_unix_socket()
 
     IRSensor.enable()
-    time.sleep(0.1)
+    time.sleep(0.5)
 
     # while True:
     #     print(IRSensor.read_all())
+    #     time.sleep(0.01)
 
-    # follow_simple()
-    # drive(5, 5)
-    # time.sleep(5)
-    # follow()
-    # follow()
-    # follow()
-    # follow()
-    # follow(0.2)
+    follow()
+    follow()
+    follow()
+    follow()
+    follow(0.2)
 
-
-main()
+main()