diff --git a/arduino_alvik/arduino_alvik.py b/arduino_alvik/arduino_alvik.py index ff5f1aa..ab23cf0 100644 --- a/arduino_alvik/arduino_alvik.py +++ b/arduino_alvik/arduino_alvik.py @@ -28,8 +28,8 @@ def __new__(cls): def __init__(self): self.i2c = _ArduinoAlvikI2C(A4, A5) self._packeter = ucPack(200) - self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L')) - self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R')) + self.left_wheel = _ArduinoAlvikWheel(self._packeter, ord('L'), alvik=self) + self.right_wheel = _ArduinoAlvikWheel(self._packeter, ord('R'), alvik=self) self._servo_positions = list((None, None,)) self.servo_A = _ArduinoAlvikServo(self._packeter, 'A', 0, self._servo_positions) self.servo_B = _ArduinoAlvikServo(self._packeter, 'B', 1, self._servo_positions) @@ -394,17 +394,22 @@ def set_wheels_speed(self, left_speed: float, right_speed: float, unit: str = 'r self._packeter.packetC2F(ord('J'), left_speed, right_speed) uart.write(self._packeter.msg[0:self._packeter.msg_size]) - def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg'): + def set_wheels_position(self, left_angle: float, right_angle: float, unit: str = 'deg', blocking: bool = True): """ Sets left/right motor angle :param left_angle: :param right_angle: :param unit: the speed unit of measurement (default: 'rpm') + :param blocking: :return: """ - self._packeter.packetC2F(ord('A'), convert_angle(left_angle, unit, 'deg'), - convert_angle(right_angle, unit, 'deg')) + left_angle = convert_angle(left_angle, unit, 'deg') + right_angle = convert_angle(right_angle, unit, 'deg') + self._packeter.packetC2F(ord('A'), left_angle, right_angle) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + self._waiting_ack = ord('P') + if blocking: + self._wait_for_target(idle_time=(max(left_angle, right_angle) / MOTOR_CONTROL_DEG_S)) def get_wheels_position(self, unit: str = 'deg') -> (float | None, float | None): """ @@ -1465,12 +1470,14 @@ def get_position(self) -> int: class _ArduinoAlvikWheel: - def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM): + def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEEL_DIAMETER_MM, + alvik: ArduinoAlvik = None): self._packeter = packeter self._label = label self._wheel_diameter_mm = wheel_diameter_mm self._speed = None self._position = None + self._alvik = alvik def reset(self, initial_position: float = 0.0, unit: str = 'deg'): """ @@ -1538,16 +1545,27 @@ def get_position(self, unit: str = 'deg') -> float | None: """ return convert_angle(self._position, 'deg', unit) - def set_position(self, position: float, unit: str = 'deg'): + def set_position(self, position: float, unit: str = 'deg', blocking: bool = True): """ Sets left/right motor speed - :param position: the speed of the motor + :param position: the position of the motor :param unit: the unit of measurement + :param blocking: :return: """ - self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), - convert_angle(position, unit, 'deg')) + position = convert_angle(position, unit, 'deg') + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('P'), position) uart.write(self._packeter.msg[0:self._packeter.msg_size]) + self._alvik._waiting_ack = ord('P') + if blocking: + self._alvik._wait_for_target(idle_time=(position / MOTOR_CONTROL_DEG_S)) + + def is_target_reached(self): + """ + Checks if the target position is reached + :return: + """ + return self._alvik.is_target_reached() class _ArduinoAlvikRgbLed: diff --git a/examples/pose_example.py b/examples/pose_example.py index ab248e9..11b675a 100644 --- a/examples/pose_example.py +++ b/examples/pose_example.py @@ -32,40 +32,37 @@ print("___________NON-BLOCKING__________________") alvik.move(50.0, 'mm', blocking=False) - print("on target after move") while not alvik.is_target_reached(): alvik.left_led.set_color(1, 0, 0) sleep_ms(500) alvik.left_led.set_color(0, 0, 0) sleep_ms(500) + print("on target after move") alvik.rotate(45.0, 'deg', blocking=False) - print("on target after rotation") - while not alvik.is_target_reached(): alvik.left_led.set_color(1, 0, 0) sleep_ms(500) alvik.left_led.set_color(0, 0, 0) sleep_ms(500) + print("on target after rotation") alvik.move(100.0, 'mm', blocking=False) - print("on target after move") - while not alvik.is_target_reached(): alvik.left_led.set_color(1, 0, 0) sleep_ms(500) alvik.left_led.set_color(0, 0, 0) sleep_ms(500) + print("on target after move") alvik.rotate(-90.00, 'deg', blocking=False) - print("on target after rotation") - while not alvik.is_target_reached(): alvik.left_led.set_color(1, 0, 0) sleep_ms(500) alvik.left_led.set_color(0, 0, 0) sleep_ms(500) + print("on target after rotation") x, y, theta = alvik.get_pose() print(f'Current pose is x(cm)={x}, y(cm)={y}, theta(deg)={theta}') diff --git a/examples/wheels_position.py b/examples/wheels_position.py index b4220dd..6f3a151 100644 --- a/examples/wheels_position.py +++ b/examples/wheels_position.py @@ -1,5 +1,5 @@ from arduino_alvik import ArduinoAlvik -from time import sleep +from time import sleep, sleep_ms import sys alvik = ArduinoAlvik() @@ -10,26 +10,55 @@ while True: try: + + alvik.set_wheels_position(45, 45) + print(f'Left wheel degs: {alvik.left_wheel.get_position()}') + print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + alvik.left_wheel.set_position(30) - sleep(2) print(f'Left wheel degs: {alvik.left_wheel.get_position()}') print(f'Right wheel degs: {alvik.right_wheel.get_position()}') alvik.right_wheel.set_position(10) - sleep(2) print(f'Left wheel degs: {alvik.left_wheel.get_position()}') print(f'Right wheel degs: {alvik.right_wheel.get_position()}') alvik.left_wheel.set_position(180) - sleep(2) print(f'Left wheel degs: {alvik.left_wheel.get_position()}') print(f'Right wheel degs: {alvik.right_wheel.get_position()}') alvik.right_wheel.set_position(270) - sleep(2) print(f'Left wheel degs: {alvik.left_wheel.get_position()}') print(f'Right wheel degs: {alvik.right_wheel.get_position()}') + print("___________NON-BLOCKING__________________") + + alvik.set_wheels_position(90, 90, blocking=False) + while not alvik.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + alvik.right_led.set_color(1, 0, 0) + sleep_ms(200) + alvik.left_led.set_color(0, 0, 0) + alvik.right_led.set_color(0, 0, 0) + sleep_ms(200) + print(f'Wheels position reached: R:{alvik.right_wheel.get_position()} L:{alvik.left_wheel.get_position()}') + + alvik.left_wheel.set_position(180, blocking=False) + while not alvik.left_wheel.is_target_reached(): + alvik.left_led.set_color(1, 0, 0) + sleep_ms(200) + alvik.left_led.set_color(0, 0, 0) + sleep_ms(200) + print(f'Left wheel position reached: {alvik.left_wheel.get_position()}') + + alvik.right_wheel.set_position(180, blocking=False) + while not alvik.right_wheel.is_target_reached(): + alvik.right_led.set_color(1, 0, 0) + sleep_ms(200) + alvik.right_led.set_color(0, 0, 0) + sleep_ms(200) + print(f'Left wheel position reached: {alvik.right_wheel.get_position()}') + except KeyboardInterrupt as e: print('over') alvik.stop()