diff --git a/firmware/README.md b/firmware/README.md index eb94159..2496ff4 100644 --- a/firmware/README.md +++ b/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): diff --git a/firmware/main/assets/calibrate_mid_gui.png b/firmware/main/assets/calibrate_mid_gui.png new file mode 100644 index 0000000..5bd728b Binary files /dev/null and b/firmware/main/assets/calibrate_mid_gui.png differ diff --git a/firmware/main/assets/calibrate_mid_spool_direction.jpg b/firmware/main/assets/calibrate_mid_spool_direction.jpg new file mode 100644 index 0000000..48db9f8 Binary files /dev/null and b/firmware/main/assets/calibrate_mid_spool_direction.jpg differ diff --git a/firmware/main/bin/firmware_calibrate_mid_pr_dd91e5a_righthand.bin b/firmware/main/bin/firmware_calibrate_mid_pr_dd91e5a_righthand.bin new file mode 100644 index 0000000..ee86a00 Binary files /dev/null and b/firmware/main/bin/firmware_calibrate_mid_pr_dd91e5a_righthand.bin differ diff --git a/firmware/main/firmware.ino b/firmware/main/firmware.ino index 25cfe58..eb980f5 100644 --- a/firmware/main/firmware.ino +++ b/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; diff --git a/sdk/src/aero_open_sdk/aero_hand.py b/sdk/src/aero_open_sdk/aero_hand.py index 34ab8be..6a73771 100644 --- a/sdk/src/aero_open_sdk/aero_hand.py +++ b/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("