Merge pull request #7 from iory/refactor

Refactor rcb4_servo_positions. Rename it

rcb4/armh7interface.py

@@ -13,9 +13,9 @@
 import serial.tools.list_ports
 
 from rcb4.asm import encode_servo_ids_to_5bytes_bin
+from rcb4.asm import encode_servo_positions_to_bytes
 from rcb4.asm import four_bit_to_num
 from rcb4.asm import rcb4_checksum
-from rcb4.asm import rcb4_servo_positions
 from rcb4.asm import rcb4_servo_svector
 from rcb4.asm import rcb4_velocity
 from rcb4.ctype_utils import c_type_to_numpy_format
@@ -375,9 +375,14 @@ def read_jointbase_sensor_ids(self):
             self.id_vector = list(sorted(ret))
         return self.id_vector
 
-    def reference_angle_vector(self):
-        return self.read_cstruct_slot_vector(
-            ServoStruct, slot_name='ref_angle')
+    def reference_angle_vector(self, servo_ids=None):
+        if servo_ids is None:
+            servo_ids = self.search_servo_ids()
+        if len(servo_ids) == 0:
+            return np.empty(shape=0)
+        ref_angles = self.read_cstruct_slot_vector(
+            ServoStruct, slot_name='ref_angle')[servo_ids]
+        return ref_angles
 
     def servo_id_to_index(self, servo_ids=None):
         if servo_ids is None:
@@ -409,6 +414,7 @@ def search_servo_ids(self):
             servo = self.memory_cstruct(ServoStruct, idx)
             if servo.flag > 0:
                 indices.append(idx)
+        indices = np.array(indices)
         self.servo_sorted_ids = indices
         return indices
 
@@ -440,7 +446,7 @@ def servo_angle_vector(self, servo_ids, servo_vector, velocity=1000):
         byte_list = [CommandTypes.MultiServoSingleVelocity.value] \
             + encode_servo_ids_to_5bytes_bin(servo_ids) \
             + [rcb4_velocity(velocity)] \
-            + rcb4_servo_positions(servo_ids, servo_vector)
+            + encode_servo_positions_to_bytes(servo_vector)
         byte_list.insert(0, 2 + len(byte_list))
         byte_list.append(rcb4_checksum(byte_list))
         return self.serial_write(byte_list)

rcb4/asm.py

@@ -1,5 +1,6 @@
 from typing import List
-from typing import Union
+
+import numpy as np
 
 
 def rcb4_checksum(byte_list: List[int]) -> int:
@@ -22,7 +23,7 @@ def rcb4_checksum(byte_list: List[int]) -> int:
 
 
 def rcb4_velocity(v):
-    return min(255, int(round(v)))
+    return max(1, min(255, int(round(v))))
 
 
 def encode_servo_ids_to_nbytes_bin(
@@ -94,31 +95,27 @@ def encode_servo_ids_to_5bytes_bin(ids: List[int]) -> List[int]:
     return encode_servo_ids_to_nbytes_bin(ids, 5)
 
 
-def rcb4_servo_positions(
-        ids: Union[int, List[int]], fvector: List[float]) -> List[int]:
-    """Creates a buffer with servo positions from given ids and float vector.
+def encode_servo_positions_to_bytes(fvector: List[float]) -> List[int]:
+    """Creates a buffer with servo positions from a float vector.
+
+    Each element in fvector should be in the range 0 to 0xFFFF (65535).
+    This range corresponds to the typical range for a 16-bit integer.
 
     Parameters
     ----------
-    ids : list or similar
-        A list of ids corresponding to servo positions.
-    fvector : list
+    fvector : List[float]
         A list of floating-point values representing servo positions.
 
     Returns
     -------
-    list
-        A list of bytes representing the low and high bytes of servo positions.
+    bytes: List[int]
+        A bytes object representing the low and high bytes of servo positions.
     """
-    if not isinstance(ids, list):
-        ids = list(ids)
-
-    fv = [int(round(v)) for v in fvector]
-    buf = []
-    for d in fv:
-        buf.append(d & 0xff)
-        buf.append((d >> 8) & 0xff)
-    return buf
+    # Ensure all values are within the 0 to 0xFFFF range
+    fvector = np.clip(fvector, 0, 0xFFFF)
+
+    int_positions = np.round(fvector).astype(np.int16)
+    return list(int_positions.tobytes())
 
 
 def four_bit_to_num(lst: List[int], values: List[int]):

tests/rcb4_tests/test_armh7interface.py

@@ -2,6 +2,7 @@
 import unittest
 
 import numpy as np
+from numpy import testing
 
 from rcb4.armh7interface import ARMH7Interface
 
@@ -19,10 +20,16 @@ def setUpClass(cls):
     def test_servo_angle_vector(self):
         self.interface.hold()
         self.interface.neutral()
+        reference = [8000, 8000]
+
         self.interface.servo_angle_vector(
             [32, 34],
-            [8000, 8000],
-            velocity=0)
+            reference,
+            velocity=1)
+        time.sleep(1.0)
+        testing.assert_array_almost_equal(
+            self.interface.reference_angle_vector(),
+            reference)
         time.sleep(4.0)
         if np.any(np.abs(self.interface.angle_vector() - 16) > 2.0):
             self.assertRaises()