diff --git a/calibrate.py b/calibrate.py index ff1f00f..e4efece 100644 --- a/calibrate.py +++ b/calibrate.py @@ -65,7 +65,6 @@ def main(): calibrated[i] = False wheels = function(arlo, *values[i]) fraction = max(wheels)/min(wheels) - print(fraction) if fraction <= THRESHOLD: calibrated[i] = True elif wheels[0] < wheels[1]: @@ -89,7 +88,6 @@ def main(): time[2] = int((1000 * cpr * cpc)/(cps[2] * 36)) # milliseconds per 10 degrees anticlockwise time[3] = int((1000 * cpr * cpc)/(cps[3] * 36)) - print(cps) values_hex = "-".join( [".".join([format(i, "x") for i in v]) for v in values] diff --git a/robot/arlo.py b/robot/arlo.py new file mode 100644 index 0000000..b22a9f2 --- /dev/null +++ b/robot/arlo.py @@ -0,0 +1,137 @@ +from time import sleep +from .robot 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())) + ) + +class Arlo(): + def __init__(self, calibration_code = None) -> None: + self.robot = Robot() + if calibration_code is None: + self._calibrate() + else: + self.calibration_code = calibration_code + self._decode_calibration_code() + + def forward(self, distance) -> None: + left, right = tuple(self.wheel_values[0]) + self.robot.go_diff(left, right, 1, 1) + sleep((distance * self.timing[0])/1000) + self.robot.stop() + + def backwards(self, distance) -> None: + left, right = tuple(self.wheel_values[1]) + self.robot.go_diff(left, right, 0, 0) + sleep((distance * self.timing[1])/1000) + self.robot.stop() + + def clockwise(self, angle) -> None: + left, right = tuple(self.wheel_values[2]) + self.robot.go_diff(left, right, 1, 0) + sleep((angle * self.timing[2])/1000) + self.robot.stop() + + def anticlockwise(self, angle) -> None: + left, right = tuple(self.wheel_values[3]) + self.robot.go_diff(left, right, 0, 1) + sleep((angle * self.timing[3])/1000) + self.robot.stop() + + def _decode_calibration_code(self) -> None: + wheel_values_hex, timing_hex = tuple(self.calibration_code.split("_")) + self.wheel_values = [ + [int(x, 16) for x in values.split(".")] + for values in wheel_values_hex.split("-") + ] + self.timing = [int(x, 16) for x in timing_hex.split("-")] + + def _calibrate(self) -> None: + 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 + ] + + while not all(calibrated): + print(calibrated, values) + for i, function in enumerate(tests): + calibrated[i] = False + wheels = function(self.robot, *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 * cpc)/cps[0]) + # milliseconds per 10cm backwards + time[1] = int((1000 * cpc)/cps[1]) + + cpr = 3.1415*38 # 1 rotation in cm + # milliseconds per 1 degrees clockwise + time[2] = int((1000 * cpr * cpc)/(cps[2] * 360)) + # milliseconds per 1 degrees anticlockwise + time[3] = int((1000 * cpr * cpc)/(cps[3] * 360)) + + self.wheel_values = values + self.timing = time + + 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]) + self.calibration_code = f"{values_hex}_{time_hex}"