✨

calibrate.py

@@ -1,8 +1,8 @@
 from time import sleep
 import robot
 
-START_VALUES = [50, 50]
-THRESHOLD = 1.005
+START_VALUES = [60, 60]
+THRESHOLD = 1
 SLEEP_TIME = 2
 
 def test_forward(arlo, l_power, r_power):
@@ -48,6 +48,7 @@ def test_anticlockwise(arlo, l_power, r_power):
 
 def main():
     values = [START_VALUES.copy() for _ in range(4)]
+    cps = [0 for _ in range(4)]
     calibrated = [False for _ in range(4)]
     tests = [
         test_forward,
@@ -63,14 +64,32 @@ def main():
             if not calibrated[i]:
                 wheels = function(arlo, *values[i])
                 fraction = max(wheels)/min(wheels)
-                if fraction < THRESHOLD:
+                if fraction <= THRESHOLD:
                     calibrated[i] = True
                 elif wheels[0] < wheels[1]:
                     values[i][0] += 1
                 else:
                     values[i][1] += 1
 
-    print(values)
+                cps[i] = wheels[0]/SLEEP_TIME
+
+    time = [0 for _ in range(4)]
+
+    cpc = 3.14159*15*144 # wheel counts per cm
+
+    # milliseconds per 10cm forward
+    time[0] = (1000 * 10 * cpc)/cps[0]
+    # milliseconds per 10cm backwards
+    time[1] = (1000 * 10 * cpc)/cps[1]
+
+    cpr = 3.1415*38 # 1 rotation in cm
+    # milliseconds per 10 degrees clockwise
+    time[2] = (1000 * cpc)/(cps[2] * 36)
+    # milliseconds per 10 degrees anticlockwise
+    time[3] = (1000 * cpc)/(cps[3] * 36)
+
+
+    print(values, time)