97 lines
2.3 KiB
Python
97 lines
2.3 KiB
Python
from time import sleep
|
|
import robot
|
|
|
|
START_VALUES = [60, 60]
|
|
THRESHOLD = 1
|
|
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)]
|
|
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):
|
|
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 = 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)
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |