From 15580df0f0aed568630619549b84241cdb7f8681 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 15:32:25 +0100 Subject: [PATCH 1/3] feat: reset/update/get wheels position wheels_servo.py --- arduino_alvik.py | 16 ++++++++++++++-- examples/wheels_servo.py | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 50 insertions(+), 2 deletions(-) create mode 100644 examples/wheels_servo.py diff --git a/arduino_alvik.py b/arduino_alvik.py index eb59935..d35213c 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -276,6 +276,9 @@ def _parse_message(self) -> int: elif code == ord('q'): # imu position _, self.roll, self.pitch, self.yaw = self.packeter.unpacketC3F() + if code == ord('w'): + # wheels position + _, self.left_wheel._position, self.right_wheel._position = self.packeter.unpacketC2F() elif code == 0x7E: # firmware version _, *self.version = self.packeter.unpacketC3B() @@ -390,6 +393,7 @@ def __init__(self, packeter: ucPack, label: int, wheel_diameter_mm: float = WHEE self._label = label self._wheel_diameter_mm = wheel_diameter_mm self._speed = None + self._position = None def reset(self, initial_position: float = 0.0): """ @@ -397,7 +401,8 @@ def reset(self, initial_position: float = 0.0): :param initial_position: :return: """ - pass + self._packeter.packetC2B1F(ord('W'), self._label & 0xFF, ord('Z'), initial_position) + uart.write(self._packeter.msg[0:self._packeter.msg_size]) def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAULT, kd: float = MOTOR_KD_DEFAULT): """ @@ -408,7 +413,7 @@ def set_pid_gains(self, kp: float = MOTOR_KP_DEFAULT, ki: float = MOTOR_KI_DEFAU :return: """ - self._packeter.packetC1B3F(ord('P'), self._label, kp, ki, kd) + self._packeter.packetC1B3F(ord('P'), self._label & 0xFF, kp, ki, kd) uart.write(self._packeter.msg[0:self._packeter.msg_size]) def stop(self): @@ -441,6 +446,13 @@ def get_speed(self) -> float: """ return self._speed + def get_position(self) -> float: + """ + Returns the wheel position (angle with respect to the reference) + :return: + """ + return self._position + def set_position(self, position: float, unit: str = 'deg'): """ Sets left/right motor speed diff --git a/examples/wheels_servo.py b/examples/wheels_servo.py new file mode 100644 index 0000000..b4220dd --- /dev/null +++ b/examples/wheels_servo.py @@ -0,0 +1,36 @@ +from arduino_alvik import ArduinoAlvik +from time import sleep +import sys + +alvik = ArduinoAlvik() +alvik.begin() + +alvik.left_wheel.reset() +alvik.right_wheel.reset() + +while True: + try: + 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()}') + + except KeyboardInterrupt as e: + print('over') + alvik.stop() + sys.exit() From 92405850bfc28964e2beb5d06fa47f9dba2cc6f7 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 15:39:41 +0100 Subject: [PATCH 2/3] message_reader.py fix: reading wrong wheel add: wheels position --- examples/message_reader.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/message_reader.py b/examples/message_reader.py index ac5b49b..2d367f9 100644 --- a/examples/message_reader.py +++ b/examples/message_reader.py @@ -12,7 +12,9 @@ try: print(f'VER: {alvik.version}') print(f'LSP: {alvik.left_wheel.get_speed()}') - print(f'RSP: {alvik.left_wheel.get_speed()}') + print(f'RSP: {alvik.right_wheel.get_speed()}') + print(f'LPOS: {alvik.left_wheel.get_position()}') + print(f'RPOS: {alvik.right_wheel.get_position()}') print(f'TOUCH: {alvik.touch_bits}') print(f'RGB: {alvik.red} {alvik.green} {alvik.blue}') print(f'LINE: {alvik.left_line} {alvik.center_line} {alvik.right_line}') From b94c5f79f6a408e83452874f92b88ca18fd9ee08 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 26 Jan 2024 15:42:27 +0100 Subject: [PATCH 3/3] mod: begin turn on illuminator --- arduino_alvik.py | 1 + 1 file changed, 1 insertion(+) diff --git a/arduino_alvik.py b/arduino_alvik.py index d35213c..c52a611 100644 --- a/arduino_alvik.py +++ b/arduino_alvik.py @@ -57,6 +57,7 @@ def begin(self) -> int: sleep_ms(100) self._reset_hw() sleep_ms(1000) + self.set_illuminator(True) return 0 def _begin_update_thread(self):