✨

calibrate.py

@@ -0,0 +1,65 @@
+from time import sleep
+import robot
+
+START_VALUES = (50, 50)
+THRESHOLD = 1.01
+
+def test_forward(arlo, l_power, r_power):
+    arlo.reset_encoder_counts()
+    arlo.go_diff(l_power, r_power, 1, 1)
+    sleep(1)
+    arlo.stop()
+    return arlo.read_left_wheel_encoder(), arlo.read_right_wheel_encoder()
+
+def test_back(arlo, l_power, r_power):
+    arlo.reset_encoder_counts()
+    arlo.go_diff(l_power, r_power, 0, 0)
+    sleep(1)
+    arlo.stop()
+    return arlo.read_left_wheel_encoder(), arlo.read_right_wheel_encoder()
+
+
+def test_clockwise(arlo, l_power, r_power):
+    arlo.reset_encoder_counts()
+    arlo.go_diff(l_power, r_power, 1, 0)
+    sleep(1)
+    arlo.stop()
+    return arlo.read_left_wheel_encoder(), arlo.read_right_wheel_encoder()
+
+def test_anticlockwise(arlo, l_power, r_power):
+    arlo.reset_encoder_counts()
+    arlo.go_diff(l_power, r_power, 0, 1)
+    sleep(1)
+    arlo.stop()
+    return arlo.read_left_wheel_encoder(), arlo.read_right_wheel_encoder()
+
+def main():
+    values = [START_VALUES for _ in range(4)]
+    calibrated = [False for _ in range(4)]
+    tests = [
+        test_forward,
+        test_back,
+        test_clockwise,
+        test_anticlockwise
+    ]
+
+    arlo = robot.Robot()
+
+    while not all(calibrated):
+        for i, function in enumerate(tests):
+            if not calibrated[i]:
+                wheels = function(arlo, *values)
+                fraction = max(wheels)/min(wheels)
+                if fraction < THRESHOLD:
+                    calibrated[i] = True
+                elif wheels[0] < wheels[1]:
+                    values[i][0] += 1
+                else:
+                    values[i][1] += 1
+
+    print(values)
+
+
+
+if __name__ == "__main__":
+    main()