Add manual servo offset calibration for tendon setup variations

firmware/README.md

@@ -170,7 +170,7 @@ You can switch hands either by editing `HandConfig.h` **or** using build flags.
 ### Frame Structure (TX and RX)
 | Bytes     | Field            | Description                                                                 |
 |-----------|-----------------|-----------------------------------------------------------------------------|
-| 0         | **Opcode**      | Command / response code (e.g., `0x01` for HOMING, `0x04` for TRIM).         |
+| 0         | **Opcode**      | Command / response code (e.g., `0x01` for HOMING, `0x03` for TRIM).         |
 | 1         | **Filler**      | Always `0x00` (reserved for future use).                                    |
 | 2..15     | **Payload**     | 14-byte payload. May contain parameters (channels, degrees, IDs, etc.) or be all zeros in acknowledgments. |
 
@@ -181,7 +181,8 @@ You can switch hands either by editing `HandConfig.h` **or** using build flags.
 | -----: | ---------- | --------- | ----------------------------------------------------------------------------------------- | ---------------------------------------------------------- |
 | `0x01` | `HOMING`   | H→D       | 14 × `0x00`                                                                               | `[0x01,0x00, 14×0x00]` when complete                       |
 | `0x02` | `SET_ID`   | H→D       | `new_id(u16)`, `current_limit(u16)`, rest zeros                                           | `[0x02,0x00, oldId(u16), newId(u16), curLim(u16), rest 0]` |
-| `0x03` | `TRIM`     | H→D       | `channel(u16:0..6)`, `degrees(i16: ±360)`, rest zeros                                     | `[0x03,0x00, channel(u16), extendCount(u16), rest 0]`      |
+| `0x03` | `TRIM`     | H→D       | `channel(u16:0..6)`, `degrees(i16: ±360)`, rest zeros                                     | `[0x03,0x00, channel(u16), extendRaw(u16), presentRaw(u16), status(u16), version(u16), rest 0]` |
+| `0x04` | `CALIBRATE_MID` | H→D   | `channel(u16:0..6)`, rest zeros                                                           | `[0x04,0x00, channel(u16), presentRaw(u16), presentU16(u16), status(u16), version(u16), rest 0]` |
 | `0x11` | `CTRL_POS` | H→D       | **7×** `u16` (channels 0..6). Range `0..65535` maps to **extend→grasp** span per channel. | *(none)*                                                   |
 | `0x12` | `CTRL_TOR` | H→D       | Set torque for all servos (7×u16)                                                     | *(none)*                                                   |
 | `0x22` | `GET_POS`  | H↔D       | 14×`0x00`                                                                                 | **7×** `u16` raw positions (counts 0..4095)                |
@@ -217,7 +218,27 @@ Direction is handled via servo_direction. Final writes use bus-batched SyncWrite
 
 **7.4 Persistence, Homing & Timing**
 
-TRIM updates the extend endpoint for one channel and saves it in NVS (persists across reboots).
+TRIM updates the extend endpoint for one channel and saves it in NVS (persists across reboots). Its ACK also reports the saved `extendRaw` and the actuator's current `presentRaw` position.
+
+### 7.4.1 Manual middle calibration
+
+`CALIBRATE_MID` calibrates the selected actuator's current physical position as its middle position. This is useful when tendon length, pretension, or spool winding places the useful finger travel outside the default single-turn window. After middle calibration, use TRIM to align the extend endpoint.
+
+![Calibrate Mid GUI](main/assets/calibrate_mid_gui.png)
+
+A real assembly can have a different tendon/spool winding direction or initial wrap than the default homing assumption:
+
+![Tendon spool direction example](main/assets/calibrate_mid_spool_direction.jpg)
+
+Safe procedure:
+
+1. Set a low speed and torque limit for the selected channel.
+2. Use the slider to place the actuator at the desired physical reference position.
+3. Press **Calibrate Mid**, select the channel, and confirm the warning.
+4. Check that the returned `presentRaw` is near the actuator midpoint (approximately 2048).
+5. Use **Trim Servo** to adjust the extend endpoint; its ACK prints both `extendRaw` and `presentRaw`.
+
+The slider remains unchanged during calibration. When position streaming resumes, the actuator may move because the same slider target is interpreted in the new coordinate system. Running HOMING later recalibrates the actuator offset again and can replace this manual middle calibration.
 
 HOMING:
 
@@ -249,7 +270,7 @@ RX: 02 00 oldId_lo oldId_hi 03 00 FF 03 00 00 00 00 00 00 00 00
 Trim channel 3 by −100° (0xFF9C):
 ```Payload
 TX: 03 00 03 00 9C FF 00 00 00 00 00 00 00 00 00 00
-RX: 03 00 03 00 ext_lo ext_hi 00 00 00 00 00 00 00 00 00 00
+RX: 03 00 03 00 ext_lo ext_hi pos_lo pos_hi 00 00 01 00 00 00 00 00
 ```
 
 CTRL_POS (7 channels). Example: all open (0):

firmware/main/firmware.ino

@@ -20,11 +20,14 @@ const uint8_t SERVO_IDS[7] = { 0, 1, 2, 3, 4, 5, 6 };
 
 ServoData sd[7];
 
+static inline void sendAckFrame(uint8_t header, const uint8_t* payload, size_t n);
+
 // ---- Constants for Control Code byte ----
-static const uint8_t HOMING    = 0x01;
-static const uint8_t SET_ID    = 0x02;
-static const uint8_t TRIM      = 0x03;
-static const uint8_t CTRL_POS  = 0x11;
+static const uint8_t HOMING        = 0x01;
+static const uint8_t SET_ID         = 0x02;
+static const uint8_t TRIM           = 0x03;
+static const uint8_t CALIBRATE_MID  = 0x04;
+static const uint8_t CTRL_POS       = 0x11;
 static const uint8_t CTRL_TOR  = 0x12;
 static const uint8_t GET_POS   = 0x22;
 static const uint8_t GET_VEL   = 0x23;
@@ -407,15 +410,55 @@ static bool handleTrimCmd(const uint8_t* payload) {
   prefs.begin("hand", false);
   prefs.putInt(String("ext" + String(ch)).c_str(), sd[ch].extend_count);
   prefs.end();
-  // ACK payload: ch (u16, LE), extend_count (u16, LE)
-  uint8_t ack[4];
-  ack[0] = (uint8_t)(ch & 0xFF);
-  ack[1] = (uint8_t)((ch >> 8) & 0xFF);
-  ack[2] = (uint8_t)(sd[ch].extend_count & 0xFF);
-  ack[3] = (uint8_t)((sd[ch].extend_count >> 8) & 0xFF);
-  sendAckFrame(TRIM, ack, sizeof(ack));   // 16 bytes on the wire
+  int present = -1;
+  if (gBusMux) xSemaphoreTake(gBusMux, portMAX_DELAY);
+  present = hlscl.ReadPos(SERVO_IDS[ch]);
+  if (gBusMux) xSemaphoreGive(gBusMux);
+
+  uint16_t present_raw = present < 0 ? 0xFFFF : (uint16_t)(((present % 4096) + 4096) % 4096);
+  uint16_t ack_values[7] = {
+      (uint16_t)ch, sd[ch].extend_count, present_raw, 0, 1, 0, 0};
+  sendU16Frame(TRIM, ack_values);
   return true;
 }
+
+static bool handleCalibrateMidCmd(const uint8_t* payload) {
+  uint16_t rawCh = leu_u16(payload);
+  uint16_t present_raw = 0xFFFF;
+  uint16_t present_u16 = 0;
+  uint16_t status = 1;
+
+  if (rawCh < 7 && g_currentMode == MODE_POS) {
+    uint8_t ch = (uint8_t)rawCh;
+    uint8_t servoID = SERVO_IDS[ch];
+    int present = -1;
+
+    if (gBusMux) xSemaphoreTake(gBusMux, portMAX_DELAY);
+    bool calibrated = hlscl.CalibrationOfs(servoID) != 0;
+    delay(30);
+    if (calibrated) present = hlscl.ReadPos(servoID);
+    bool mode_set = hlscl.ServoMode(servoID) != 0;
+    bool locked = hlscl.LockEprom(servoID) != 0;
+    bool position_set = false;
+    if (present >= 0 && mode_set && locked) {
+      position_set = hlscl.WritePosEx(
+          servoID, present, g_speed[ch], g_accel[ch], g_torque[ch]) != 0;
+    }
+    bool torque_enabled = hlscl.EnableTorque(servoID, 1) != 0;
+    if (gBusMux) xSemaphoreGive(gBusMux);
+
+    if (calibrated && present >= 0 && mode_set && locked && position_set && torque_enabled) {
+      present_raw = (uint16_t)(((present % 4096) + 4096) % 4096);
+      present_u16 = mapRawToU16(ch, present_raw);
+      if (present_raw >= 2000 && present_raw <= 2096) status = 0;
+    }
+  }
+
+  uint16_t ack_values[7] = {rawCh, present_raw, present_u16, status, 1, 0, 0};
+  sendU16Frame(CALIBRATE_MID, ack_values);
+  return true;
+}
+
 static bool handleSetSpeedCmd(const uint8_t* payload)
 {
   uint16_t rawId = (uint16_t)payload[0] | ((uint16_t)payload[1] << 8);
@@ -563,6 +606,10 @@ static bool handleHostFrame(uint8_t op) {
       return handleTrimCmd(payload);
     }
 
+    case CALIBRATE_MID: {
+      return handleCalibrateMidCmd(payload);
+    }
+
     case GET_POS: {
       sendPositions();
       return true;

sdk/src/aero_open_sdk/aero_hand.py

@@ -14,8 +14,9 @@
 # limitations under the License.
 
 import os
-import time 
+import time
 import struct
+import threading
 from serial import Serial, SerialTimeoutException
 from typing import Iterator
 
@@ -27,6 +28,7 @@
 HOMING_MODE = 0x01
 SET_ID_MODE = 0x02
 TRIM_MODE = 0x03
+CALIBRATE_MID_MODE = 0x04
 
 ## Command Modes
 CTRL_POS = 0x11
@@ -55,6 +57,7 @@ def __init__(self, port=None, baudrate=921600):
             print("No port specified. Attempting to auto-detect Aero Hand serial port...")
             port = self._detect_port()
         self.ser = Serial(port, baudrate, timeout=0.01, write_timeout=0.01)
+        self._serial_lock = threading.RLock()
 
         ## Clean Buffers before starting
         self.ser.reset_input_buffer()
@@ -315,12 +318,53 @@ def trim_servo(self, id: int, degrees: int):
 
         payload = [0] * 7
         payload[0] = id & 0xFFFF
-        payload[1] = degrees & 0xFFFF  
-        self._send_data(TRIM_MODE, payload)
-        payload = self._wait_for_ack(TRIM_MODE, 2.0)
-        id, extend = struct.unpack_from("<HH", payload, 0)
-        return {"Servo ID": id, "Extend Count": extend}
-
+        payload[1] = degrees & 0xFFFF
+        with self._serial_lock:
+            self._send_data(TRIM_MODE, payload)
+            payload = self._wait_for_ack(TRIM_MODE, 2.0)
+        id, extend_raw, present_raw, status, protocol_version = struct.unpack_from(
+            "<HHHHH", payload, 0
+        )
+        if status != 0:
+            raise RuntimeError("Trim failed to save the extend endpoint")
+        if protocol_version != 1:
+            present_raw = 0xFFFF
+        return {
+            "Servo ID": id,
+            "Extend Count": extend_raw,
+            "Extend Raw": extend_raw,
+            "Present Raw": None if present_raw == 0xFFFF else present_raw,
+        }
+
+    def calibrate_mid(self, id: int):
+        """Calibrate the selected actuator's current position as its middle position."""
+        if not (0 <= id <= 6):
+            raise ValueError("id must be 0..6")
+
+        payload = [0] * 7
+        payload[0] = id
+        with self._serial_lock:
+            try:
+                self.ser.reset_input_buffer()
+            except Exception:
+                pass
+            self._send_data(CALIBRATE_MID_MODE, payload)
+            payload = self._wait_for_ack(CALIBRATE_MID_MODE, 2.0)
+        servo_id, present_raw, present_u16, status, protocol_version = struct.unpack_from(
+            "<HHHHH", payload, 0
+        )
+        if protocol_version != 1:
+            raise RuntimeError("Calibrate Mid requires firmware protocol version 1")
+        if status != 0 or servo_id == 0xFFFF or present_raw == 0xFFFF:
+            raise RuntimeError(
+                "Calibrate Mid failed; verify position mode and the actuator connection"
+            )
+        return {
+            "Servo ID": servo_id,
+            "Present Raw": present_raw,
+            "Present U16": present_u16,
+        }
+
     def ctrl_torque(self, torque: list[int]):
         """
         Set the same torque value for all 7 servos using the CTRL_TOR command.
@@ -337,8 +381,9 @@ def _send_data(self, header: int, payload: list[int] = [0] * 7):
         assert len(payload) == 7, "Payload must be a list of 7 integers in Range 0-65535"
         assert all(0 <= v <= 65535 for v in payload), "Payload values must be in Range 0-65535"
         msg = struct.pack("<2B7H", header & 0xFF, 0x00, *(v & 0xFFFF for v in payload))
-        self.ser.write(msg)
-        self.ser.flush()
+        with self._serial_lock:
+            self.ser.write(msg)
+            self.ser.flush()
 
     def send_homing(self, timeout_s: float = 175.0):
         try:

sdk/src/aero_open_sdk/gui.py

@@ -44,8 +44,9 @@
 
 # ---- operation codes ------------
 HOMING_MODE = 0x01
-SET_ID_MODE = 0x03
-TRIM_MODE   = 0x04
+SET_ID_MODE = 0x02
+TRIM_MODE = 0x03
+CALIBRATE_MID_MODE = 0x04
 
 CTRL_POS = 0x11
 
@@ -167,12 +168,16 @@ def _build_ui(self):
         self.btn_get_vel  = ttk.Button(cmd, text="GET_VEL",  command=self.on_get_vel,  state=tk.DISABLED)
         self.btn_get_cur  = ttk.Button(cmd, text="GET_CURR", command=self.on_get_cur,  state=tk.DISABLED)
         self.btn_get_temp = ttk.Button(cmd, text="GET_TEMP", command=self.on_get_temp, state=tk.DISABLED)
-        self.btn_get_all  = ttk.Button(cmd, text="GET_ALL",  command=self.on_get_all,  state=tk.DISABLED)
+        self.btn_get_all = ttk.Button(cmd, text="GET_ALL", command=self.on_get_all, state=tk.DISABLED)
+        self.btn_calibrate_mid = ttk.Button(
+            cmd, text="Calibrate Mid", command=self.on_calibrate_mid, state=tk.DISABLED
+        )
         self.btn_get_pos.pack(side=tk.LEFT, padx=(20, 6))
         self.btn_get_vel.pack(side=tk.LEFT, padx=6)
         self.btn_get_cur.pack(side=tk.LEFT, padx=6)
         self.btn_get_temp.pack(side=tk.LEFT, padx=6)
         self.btn_get_all.pack(side=tk.LEFT, padx=6)
+        self.btn_calibrate_mid.pack(side=tk.LEFT, padx=6)
 
         # ---- Sliders (7)
         self.grp = ttk.LabelFrame(self, text="Sliders (send CTRL_POS payload)", padding=10)
@@ -256,6 +261,7 @@ def on_torque_control(self):
         self.set_status("Torque control mode: stopped CTRL_POS streaming")
         for scale in self.slider_widgets:
             scale.configure(state=tk.DISABLED)
+        self.btn_calibrate_mid.configure(state=tk.DISABLED)
         self.grp.configure(text="Sliders (CTRL_POS disabled)")
         # Show torque slider
         self.torque_frame.pack(side=tk.TOP, fill=tk.X, padx=10, pady=(6, 10))
@@ -277,6 +283,7 @@ def disable_torque_control(self):
         self.torque_frame.pack_forget()
         for scale in self.slider_widgets:
             scale.configure(state=tk.NORMAL)
+        self.btn_calibrate_mid.configure(state=tk.NORMAL)
         self.grp.configure(text="Sliders (send CTRL_POS payload)")
 
     # ------------- connect/disconnect -------------
@@ -306,7 +313,7 @@ def on_connect(self):
             self.btn_connect.configure(state=tk.DISABLED)
             self.btn_disc.configure(state=tk.NORMAL)
             for b in (self.btn_zero,self.btn_homing, self.btn_setid, self.btn_trim,self.btn_set_speed,self.btn_set_torque,
-                      self.btn_get_pos, self.btn_get_vel, self.btn_get_cur, self.btn_get_temp, self.btn_get_all):
+                      self.btn_get_pos, self.btn_get_vel, self.btn_get_cur, self.btn_get_temp, self.btn_get_all, self.btn_calibrate_mid):
                 b.configure(state=tk.NORMAL)
 
             self.set_status(f"Connected to {port} @ {baud}")
@@ -340,7 +347,7 @@ def _shutdown_serial(self):
         self.btn_connect.configure(state=tk.NORMAL)
         self.btn_disc.configure(state=tk.DISABLED)
         for b in (self.btn_zero,self.btn_homing, self.btn_setid, self.btn_trim,self.btn_set_speed,self.btn_set_torque,
-                  self.btn_get_pos, self.btn_get_vel, self.btn_get_cur, self.btn_get_temp, self.btn_get_all):
+                  self.btn_get_pos, self.btn_get_vel, self.btn_get_cur, self.btn_get_temp, self.btn_get_all, self.btn_calibrate_mid):
             b.configure(state=tk.DISABLED)
         self.set_status("Disconnected")
         self.log("[info] Disconnected")
@@ -510,8 +517,11 @@ def worker():
                 self.control_paused = True
                 self.set_status("Trimming… waiting for ACK")
                 self.log(f"[TX] TRIM sent (ch={ch}, deg={deg})")
-                ack = self.hand.trim_servo(ch, deg)  # dict with Servo ID, Extend Count
-                self.log(f"[ACK] TRIM: id={ack['Servo ID']} extend={ack['Extend Count']}")
+                ack = self.hand.trim_servo(ch, deg)
+                self.log(
+                    f"[ACK] TRIM: id={ack['Servo ID']} "
+                    f"extend_raw={ack['Extend Raw']} present_raw={ack['Present Raw']}"
+                )
                 self.set_status("Trim complete")
             except Exception as e:
                 self.log(f"[err] TRIM failed: {e}")
@@ -521,6 +531,46 @@ def worker():
 
         threading.Thread(target=worker, daemon=True).start()
 
+    def on_calibrate_mid(self):
+        if not self.hand:
+            return
+        ch = simpledialog.askinteger(
+            "Calibrate Mid", "Servo ID / channel (0..6):",
+            minvalue=0, maxvalue=6, parent=self,
+        )
+        if ch is None:
+            return
+        confirmed = messagebox.askyesno(
+            "Calibrate Mid",
+            "The selected actuator's current physical position will become its middle position.\n\n"
+            "The slider will stay unchanged. When position streaming resumes, the actuator may move "
+            "to the same slider target in the new coordinate system.\n\n"
+            "Set low speed and torque before continuing. Proceed?",
+            parent=self,
+        )
+        if not confirmed:
+            return
+
+        def worker():
+            try:
+                self.control_paused = True
+                self.set_status("Calibrating middle position… waiting for ACK")
+                self.log(f"[TX] CALIBRATE_MID sent (ch={ch}, slider={self.slider_vars[ch].get():.3f})")
+                ack = self.hand.calibrate_mid(ch)
+                self.log(
+                    f"[ACK] CALIBRATE_MID: id={ack['Servo ID']} "
+                    f"present_raw={ack['Present Raw']} present_u16={ack['Present U16']} "
+                    f"slider_unchanged={self.slider_vars[ch].get():.3f}"
+                )
+                self.set_status("Middle calibration complete; resuming unchanged slider target")
+            except Exception as e:
+                self.log(f"[err] CALIBRATE_MID failed: {e}")
+                self.set_status("Middle calibration failed")
+            finally:
+                self.control_paused = False
+
+        threading.Thread(target=worker, daemon=True).start()
+
     # ---- GET_* buttons (request + show parsed reply) ----
     def on_get_pos(self):
         if not self.hand: