feat: reset/update/get wheels position

wheels_servo.py

Author: eigen-value

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,14 +393,16 @@ 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):
         """
         Resets the wheel reference position
         :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

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()