From 477afa74c0cf50f1347acc86ab58b60f3dab96e8 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Fri, 1 Nov 2024 22:18:17 +0900
Subject: [PATCH] Estimate servo current from kondo reference manuals

---
 rcb4/armh7interface.py | 29 ++++++++++++++++++++++++++++-
 rcb4/rcb4interface.py  | 38 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 66 insertions(+), 1 deletion(-)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index 1107e15b..c894b158 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -26,11 +26,12 @@
 from rcb4.data import kondoh7_elf
 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_temperatures
 from rcb4.rcb4interface import rcb4_dof
 from rcb4.rcb4interface import RCB4Interface
 from rcb4.rcb4interface import ServoOnOffValues
 from rcb4.rcb4interface import ServoParams
-from rcb4.rcb4interface import interpolate_or_extrapolate_temperatures
 from rcb4.struct_header import c_vector
 from rcb4.struct_header import DataAddress
 from rcb4.struct_header import GPIOStruct
@@ -1409,6 +1410,32 @@ def servo_states(self):
             return servo_on_ids
         return []
 
+    def switch_reading_servo_crreunt(self, enable=True):
+        if enable:
+            value = 1
+        else:
+            value = 0
+        return self.write_cstruct_slot_v(SystemStruct, "servo_current_read_flag", [value])
+
+    def read_servo_current(self):
+        servo_ids = self.search_servo_ids()
+        if len(servo_ids) == 0:
+            return np.zeros(shape=0)
+
+        current_vector = self.read_cstruct_slot_vector(ServoStruct, "current")
+        servo_current_vector = current_vector[servo_ids]
+
+        # Create a mask for non-zero values (i.e., values that are valid and need processing)
+        non_zero_mask = servo_current_vector != 64
+        servo_current_vector[servo_current_vector == 64] = 0
+
+        # Apply calculations only on non-zero values
+        estimated_current_vector = np.zeros_like(servo_current_vector)
+        estimated_current_vector[non_zero_mask] = interpolate_currents(
+            servo_current_vector[non_zero_mask]
+        )
+        return estimated_current_vector
+
     def switch_reading_servo_temperature(self, enable=True):
         if enable:
             value = 1
diff --git a/rcb4/rcb4interface.py b/rcb4/rcb4interface.py
index 4ed50eb2..93787d69 100644
--- a/rcb4/rcb4interface.py
+++ b/rcb4/rcb4interface.py
@@ -17,6 +17,10 @@
 temperature_data = np.array([100, 90, 80, 70, 60])
 setting_values_data = np.array([30, 47, 60, 75, 87])
 
+# Known data points for current and setting values
+current_data = np.array([0.0, 0.1, 0.5, 1.0, 1.5, 2.0])  # in Amps
+setting_values_data = np.array([0, 1, 5, 10, 15, 20])  # setting values
+
 
 def interpolate_or_extrapolate_temperatures(setting_values):
     """Approximation of temperatures for an array of setting values."""
@@ -54,6 +58,40 @@ def interpolate_or_extrapolate_temperatures(setting_values):
     return temperatures
 
 
+def interpolate_currents(setting_values):
+    """Approximation of current values for an array of setting values."""
+    # Calculate slopes for interpolation between points
+    slopes = (current_data[1:] - current_data[:-1]) / (
+        setting_values_data[1:] - setting_values_data[:-1]
+    )
+
+    # Find indices where each setting value would fit within setting_values_data
+    indices = np.searchsorted(setting_values_data, setting_values, side="right") - 1
+
+    # Initialize result array with zeros (or any placeholder values)
+    current_values = np.zeros_like(setting_values, dtype=float)
+
+    # Interpolate within range
+    in_range = (indices >= 0) & (indices < len(slopes))
+    current_values[in_range] = current_data[indices[in_range]] + slopes[
+        indices[in_range]
+    ] * (setting_values[in_range] - setting_values_data[indices[in_range]])
+
+    # Extrapolate for values below the minimum setting value
+    below_range = setting_values < setting_values_data[0]
+    current_values[below_range] = current_data[0] + slopes[0] * (
+        setting_values[below_range] - setting_values_data[0]
+    )
+
+    # Extrapolate for values above the maximum setting value
+    above_range = setting_values > setting_values_data[-1]
+    current_values[above_range] = current_data[-1] + slopes[-1] * (
+        setting_values[above_range] - setting_values_data[-1]
+    )
+
+    return current_values
+
+
 # 4000.0 / 135
 deg_to_servovector = 29.62962962962963