101 lines
2.6 KiB
Python
101 lines
2.6 KiB
Python
from time import sleep
|
|
import robot
|
|
|
|
START_VALUES = [60, 60]
|
|
THRESHOLD = 1.05
|
|
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 (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(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 (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(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 (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(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 (
|
|
abs(int(arlo.read_left_wheel_encoder())),
|
|
abs(int(arlo.read_right_wheel_encoder()))
|
|
)
|
|
|
|
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,
|
|
test_back,
|
|
test_clockwise,
|
|
test_anticlockwise
|
|
]
|
|
|
|
arlo = robot.Robot()
|
|
|
|
while not all(calibrated):
|
|
print(calibrated, values)
|
|
for i, function in enumerate(tests):
|
|
calibrated[i] = False
|
|
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
|
|
|
|
cps[i] = wheels[0]/SLEEP_TIME
|
|
|
|
time = [0 for _ in range(4)]
|
|
|
|
cpc = 144/(3.14159*15) # wheel counts per cm
|
|
|
|
# milliseconds per 10cm forward
|
|
time[0] = int((1000 * 10 * cpc)/cps[0])
|
|
# milliseconds per 10cm backwards
|
|
time[1] = int((1000 * 10 * cpc)/cps[1])
|
|
|
|
cpr = 3.1415*38 # 1 rotation in cm
|
|
# milliseconds per 10 degrees clockwise
|
|
time[2] = int((1000 * cpr * cpc)/(cps[2] * 36))
|
|
# milliseconds per 10 degrees anticlockwise
|
|
time[3] = int((1000 * cpr * cpc)/(cps[3] * 36))
|
|
|
|
values_hex = "-".join(
|
|
[".".join([format(i, "x") for i in v]) for v in values]
|
|
)
|
|
time_hex = "-".join([format(i, "x") for i in time])
|
|
print(f"{values_hex}_{time_hex}")
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |