78 lines
1.9 KiB
Python
78 lines
1.9 KiB
Python
from time import sleep
|
|
import robot
|
|
|
|
START_VALUES = [50, 50]
|
|
THRESHOLD = 1.005
|
|
SLEEP_TIME = 2
|
|
|
|
def test_forward(arlo, l_power, r_power):
|
|
arlo.reset_encoder_counts()
|
|
arlo.go_diff(l_power, r_power, 1, 1)
|
|
sleep(SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
int(arlo.read_left_wheel_encoder()),
|
|
int(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(SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
int(arlo.read_left_wheel_encoder()),
|
|
int(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(SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
int(arlo.read_left_wheel_encoder()),
|
|
int(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(SLEEP_TIME)
|
|
arlo.stop()
|
|
return (
|
|
int(arlo.read_left_wheel_encoder()),
|
|
int(arlo.read_right_wheel_encoder())
|
|
)
|
|
|
|
def main():
|
|
values = [START_VALUES.copy() 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[i])
|
|
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() |