From 920289acbe9372234a9b7457ea98b60242529161 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Mon, 19 Sep 2022 15:13:36 +0200 Subject: [PATCH] :sparkles: --- calibrate.py | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 calibrate.py diff --git a/calibrate.py b/calibrate.py new file mode 100644 index 0000000..64bf0bc --- /dev/null +++ b/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() \ No newline at end of file