Rename rcb4_servo_ids_to_5bytes to encode_servo_ids_to_5bytes_bin and…

… refactor it (#4)

rcb4/armh7interface.py

@@ -12,9 +12,9 @@
 import serial
 import serial.tools.list_ports
 
+from rcb4.asm import encode_servo_ids_to_5bytes_bin
 from rcb4.asm import four_bit_to_num
 from rcb4.asm import rcb4_checksum
-from rcb4.asm import rcb4_servo_ids_to_5bytes
 from rcb4.asm import rcb4_servo_positions
 from rcb4.asm import rcb4_servo_svector
 from rcb4.asm import rcb4_velocity
@@ -438,7 +438,7 @@ def neutral(self, servo_ids=None, velocity=1000):
 
     def servo_angle_vector(self, servo_ids, servo_vector, velocity=1000):
         byte_list = [CommandTypes.MultiServoSingleVelocity.value] \
-            + rcb4_servo_ids_to_5bytes(servo_ids) \
+            + encode_servo_ids_to_5bytes_bin(servo_ids) \
             + [rcb4_velocity(velocity)] \
             + rcb4_servo_positions(servo_ids, servo_vector)
         byte_list.insert(0, 2 + len(byte_list))
@@ -468,7 +468,7 @@ def send_stretch(self, value=127, servo_ids=None):
         if not isinstance(value, list) or not isinstance(value, tuple):
             value = [value] * len(servo_ids)
         byte_list = [CommandTypes.ServoParam.value] \
-            + rcb4_servo_ids_to_5bytes(servo_ids) \
+            + encode_servo_ids_to_5bytes_bin(servo_ids) \
             + [ServoParams.Stretch.value] \
             + rcb4_servo_svector(servo_ids, value)
         byte_list.insert(0, 2 + len(byte_list))

rcb4/asm.py

@@ -21,17 +21,79 @@ def rcb4_checksum(byte_list: List[int]) -> int:
     return sum(b & 0xff for b in byte_list) & 0xff
 
 
-def rcb4_servo_ids_to_5bytes(seq: List[int]) -> List[int]:
-    ids = [0, 0, 0, 0, 0]
-    for c in seq:
-        ids[c // 8] |= (1 << (c % 8))
-    return ids
-
-
 def rcb4_velocity(v):
     return min(255, int(round(v)))
 
 
+def encode_servo_ids_to_nbytes_bin(
+        ids: List[int], num_bytes: int) -> List[int]:
+    """Encode a list of servo motor IDs into a specified number of bytes.
+
+    This function takes a list of servo motor IDs (each between 0 and
+    num_bytes * 8 - 1) and encodes it into a specified number of bytes.
+    Each bit in the byte sequence represents whether a corresponding
+    servo motor is active (1) or not (0). The function then splits
+    this bit sequence into the specified number of bytes.
+
+    Parameters
+    ----------
+    ids : List[int]
+        A list of integers representing the servo motor IDs.
+        Each ID should be less than num_bytes * 8.
+    num_bytes : int
+        The number of bytes to encode the IDs into.
+
+    Returns
+    -------
+    List[int]
+        A list of integers, where each integer is
+        a byte representation (0-255) of the servo motor states.
+        The list represents the bit sequence split into the specified
+        number of bytes.
+    """
+    bit_representation = 0
+    for idx in ids:
+        bit_representation |= 1 << idx
+    return [(bit_representation >> (8 * i)) & 0xFF for i in range(num_bytes)]
+
+
+def encode_servo_ids_to_5bytes_bin(ids: List[int]) -> List[int]:
+    """Encode a list of servo motor IDs into a 5-byte representation.
+
+    This is a specialized use of the general
+    function 'encode_servo_ids_to_nbytes_bin' for encoding
+    servo motor IDs into 5 bytes. It's suitable for servo motors
+    with IDs ranging from 0 to 39.
+
+    Parameters
+    ----------
+    ids : List[int]
+        A list of integers representing the servo motor IDs.
+        Each ID should be in the range 0 to 39.
+
+    Returns
+    -------
+    List[int]
+        A list of 5 integers, each representing a byte of the servo
+        motor states.
+
+    Examples
+    --------
+    >>> encode_servo_ids_to_5bytes_bin([2, 9, 16, 23, 30])
+    [4, 2, 1, 128, 64]
+
+    The corresponding binary representation of each byte is:
+    '00000100' (for the servo with ID 2),
+    '00000010' (for the servo with ID 9),
+    '00000001' (for the servo with ID 16),
+    '10000000' (for the servo with ID 23),
+    '01000000' (for the servo with ID 30).
+
+    This means the servo motors with IDs 2, 9, 16, 23, and 30 are active.
+    """
+    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.