Add send_current_limit and send_temperature_limit

rcb4/armh7interface.py

@@ -27,6 +27,8 @@
 from rcb4.rcb4interface import CommandTypes
 from rcb4.rcb4interface import deg_to_servovector
 from rcb4.rcb4interface import interpolate_currents
+from rcb4.rcb4interface import interpolate_or_extrapolate_current_settings
+from rcb4.rcb4interface import interpolate_or_extrapolate_temperature_settings
 from rcb4.rcb4interface import interpolate_or_extrapolate_temperatures
 from rcb4.rcb4interface import rcb4_dof
 from rcb4.rcb4interface import RCB4Interface
@@ -919,6 +921,38 @@ def send_stretch(self, value=127, servo_ids=None):
         byte_list.append(rcb4_checksum(byte_list))
         return self.serial_write(byte_list)
 
+    def send_current_limit(self, current_limit_a=4.0, servo_ids=None):
+        if servo_ids is None:
+            servo_ids = self.servo_sorted_ids
+        current_setting = interpolate_or_extrapolate_current_settings(current_limit_a)
+        if not isinstance(current_setting, (list, tuple)):
+            current_setting = [current_setting] * len(servo_ids)
+        byte_list = (
+            [CommandTypes.ServoParam.value]
+            + encode_servo_ids_to_5bytes_bin(servo_ids)
+            + [ServoParams.CurrentLimit.value]
+            + rcb4_servo_svector(servo_ids, current_setting)
+        )
+        byte_list.insert(0, 2 + len(byte_list))
+        byte_list.append(rcb4_checksum(byte_list))
+        return self.serial_write(byte_list)
+
+    def send_temperature_limit(self, temperature_limit_c=80, servo_ids=None):
+        if servo_ids is None:
+            servo_ids = self.servo_sorted_ids
+        temperature_setting = interpolate_or_extrapolate_temperature_settings(temperature_limit_c)
+        if not isinstance(temperature_setting, (list, tuple)):
+            temperature_setting = [temperature_setting] * len(servo_ids)
+        byte_list = (
+            [CommandTypes.ServoParam.value]
+            + encode_servo_ids_to_5bytes_bin(servo_ids)
+            + [ServoParams.TemperatureLimit.value]
+            + rcb4_servo_svector(servo_ids, temperature_setting)
+        )
+        byte_list.insert(0, 2 + len(byte_list))
+        byte_list.append(rcb4_checksum(byte_list))
+        return self.serial_write(byte_list)
+
     def read_quaternion(self):
         cs = self.memory_cstruct(Madgwick, 0)
         return np.array([cs.q0, cs.q1, cs.q2, cs.q3], dtype=np.float32)

rcb4/rcb4interface.py

@@ -98,6 +98,81 @@ def interpolate_currents(setting_values):
     return current_values if len(current_values) > 1 else current_values[0]
 
 
+def interpolate_or_extrapolate_temperature_settings(temperatures):
+    """Approximation of setting values for an array of temperatures."""
+    temperatures = np.atleast_1d(temperatures)  # Ensure temperatures is an array
+
+    # Calculate slopes for interpolation between points
+    slopes = (temperature_setting_values_data[1:] - temperature_setting_values_data[:-1]) / (
+        temperature_data[1:] - temperature_data[:-1]
+    )
+
+    # Find indices where each temperature would fit within temperature_data
+    indices = np.searchsorted(temperature_data[::-1], temperatures, side="right") - 1
+    indices = len(temperature_data) - 1 - indices  # Reverse indices for original order
+
+    # Initialize result array with zeros (or any placeholder values)
+    setting_values = np.zeros_like(temperatures, dtype=float)
+
+    # Interpolate within range
+    in_range = (indices >= 0) & (indices < len(slopes))
+    setting_values[in_range] = temperature_setting_values_data[indices[in_range]] + slopes[
+        indices[in_range]
+    ] * (temperatures[in_range] - temperature_data[indices[in_range]])
+
+    # Extrapolate for values below the minimum temperature
+    below_range = temperatures > temperature_data[0]
+    setting_values[below_range] = temperature_setting_values_data[0] + slopes[0] * (
+        temperatures[below_range] - temperature_data[0]
+    )
+
+    # Extrapolate for values above the maximum temperature
+    above_range = temperatures < temperature_data[-1]
+    setting_values[above_range] = temperature_setting_values_data[-1] + slopes[-1] * (
+        temperatures[above_range] - temperature_data[-1]
+    )
+
+    # If input was a single value, return a single float instead of an array
+    return setting_values if len(setting_values) > 1 else setting_values[0]
+
+
+def interpolate_or_extrapolate_current_settings(currents):
+    """Approximation of setting values for an array of currents."""
+    currents = np.atleast_1d(currents)  # Ensure currents is an array
+
+    # Calculate slopes for interpolation between points
+    slopes = (current_setting_values_data[1:] - current_setting_values_data[:-1]) / (
+        current_data[1:] - current_data[:-1]
+    )
+
+    # Find indices where each current would fit within current_data
+    indices = np.searchsorted(current_data, currents, side="right") - 1
+
+    # Initialize result array with zeros (or any placeholder values)
+    setting_values = np.zeros_like(currents, dtype=float)
+
+    # Interpolate within range
+    in_range = (indices >= 0) & (indices < len(slopes))
+    setting_values[in_range] = current_setting_values_data[indices[in_range]] + slopes[
+        indices[in_range]
+    ] * (currents[in_range] - current_data[indices[in_range]])
+
+    # Extrapolate for values below the minimum current
+    below_range = currents < current_data[0]
+    setting_values[below_range] = current_setting_values_data[0] + slopes[0] * (
+        currents[below_range] - current_data[0]
+    )
+
+    # Extrapolate for values above the maximum current
+    above_range = currents > current_data[-1]
+    setting_values[above_range] = current_setting_values_data[-1] + slopes[-1] * (
+        currents[above_range] - current_data[-1]
+    )
+
+    # If input was a single value, return a single float instead of an array
+    return setting_values if len(setting_values) > 1 else setting_values[0]
+
+
 # 4000.0 / 135
 deg_to_servovector = 29.62962962962963