Stservo Debug

Key Control Table Registers (STS3215)

Error bits in STATUS: ERRBIT_VOLTAGE=1, ERRBIT_ANGLE=2, ERRBIT_OVERHEAT=4, ERRBIT_OVERELE=8, ERRBIT_OVERLOAD=32

Available sts High-Level Methods

# --- Connection ---
PortHandler(port).openPort()
PortHandler(port).setBaudRate(1_000_000)

# --- Discovery ---
servo.ping(servo_id)              # → (model, result, error)

# --- Position / Motion ---
servo.WritePosEx(id, pos, speed, acc)         # write directly
servo.RegWritePosEx(id, pos, speed, acc)      # queue for RegAction
servo.SyncWritePosEx(id, pos, speed, acc)     # add to sync group, then:
servo.groupSyncWrite.txPacket()               # fire all at once
servo.RegAction()                             # broadcast INST_ACTION
servo.WheelMode(id)                           # set continuous rotation
servo.WriteSpec(id, speed, acc)               # wheel-mode speed

# --- Telemetry reads ---
servo.ReadPos(id)           # → (pos, result, error)
servo.ReadSpeed(id)         # → (speed, result, error)
servo.ReadPosSpeed(id)      # → (pos, speed, result, error)
servo.ReadMoving(id)        # → (moving, result, error)
servo.IsMoving(id)          # → (bool, result, error)
servo.ReadLoad(id)          # → (load_pct, result, error)
servo.ReadVoltage(id)       # → (volts, result, error)
servo.ReadCurrent(id)       # → (mA, result, error)
servo.ReadTemperature(id)   # → (degC, result, error)
servo.ReadAccelaration(id)  # → (acc, result, error)
servo.ReadMode(id)          # → (mode, result, error)
servo.ReadCorrection(id)    # → (offset, result, error)
servo.ReadStatus(id)        # → (status_byte, result, error)
servo.GetBaudrate()         # → int (from PortHandler)

# --- EEPROM management ---
servo.LockEprom(id)
servo.unLockEprom(id)

# --- Direct register access (inherited from ProtocolPacketHandler) ---
servo.read1ByteTxRx(id, address)
servo.read2ByteTxRx(id, address)
servo.write1ByteTxRx(id, address, value)
servo.write2ByteTxRx(id, address, value)
servo.writeTxRx(id, address, length, data_list)

All reads return PacketResult(data, result, error) or (value, result, error). Always check result == COMM_SUCCESS (0).

Procedures

1. Connect to the Bus

from stservo_sdk import PortHandler, sts
from stservo_sdk.stservo_def import COMM_SUCCESS

ph = PortHandler("/dev/tty.usbserial-XXXX")   # macOS; Linux: /dev/ttyUSB0
assert ph.openPort(), "Failed to open port"
assert ph.setBaudRate(1_000_000), "Failed to set baud"
servo = sts(ph)

Find ports with: python -c "from serial.tools import list_ports; [print(p.device, p.description) for p in list_ports.comports()]"

2. Scan Bus for Servos

from homing_calibrate import discover_servos
found = discover_servos("/dev/tty.usbserial-XXXX", 1_000_000, range(1, 11))
# Returns {id: model_number}

Or via CLI: python homing_calibrate.py --device /dev/tty.usbserial-XXXX --scan-range 1-10

3. Read Full Diagnostics

from homing_calibrate import read_servo_diagnostics
diag = read_servo_diagnostics("/dev/tty.usbserial-XXXX", 1_000_000, [1,2,3,4,5,6])
for sid, data in diag.items():
    print(f"Servo {sid}: pos={data['Position']} V={data['Voltage']} T={data['Temperature']}")

4. Homing / Zero Calibration (soarm100)

soarm100 joint IDs: 1–6
Zero pose: a specific physical pose (e.g. arm hanging straight down / fully folded) — NOT the midpoint 2048. Position 2048 is the electrical midpoint; the physical zero pose must be manually established.

Goal: Write STS_OFS_L offset on each joint so that the desired physical zero pose reads as the agreed reference value used by higher-level modules.

Step 1  — Unlock EEPROM on all joints (IDs 1-6)
           --unlock flag or: servo.unLockEprom(id)

Step 2  — Disable torque so joints can be moved freely
           servo.write1ByteTxRx(id, STS_TORQUE_ENABLE, 0)  # repeat for 1-6

Step 3  — Manually move arm to physical zero pose

Step 4  — Record raw positions at zero pose
           pos, _, _ = servo.ReadPos(id)  # note these for each of IDs 1-6

Step 5  — Decide the target reference value for "zero" (e.g. 2048)
           Compute offset = target_zero - pos
           Encode: raw_offset = (abs(offset) | 0x0800) if offset < 0 else abs(offset)
           servo.write2ByteTxRx(id, STS_OFS_L, raw_offset)

Step 6  — Set angle limits per joint (define physical travel boundaries)

Step 7  — Re-enable torque; lock EEPROM
           servo.LockEprom(id)

Via existing CLI tools:

python homing_calibrate.py \
    --device /dev/tty.usbserial-XXXX \
    --scan-range 1-6 \
    --angle-limit 1:512:3584 --angle-limit 2:512:3584 \
    --set-acc 1:50 --set-speed 1:300 \
    --torque 1:on --unlock --lock

5. Configure a New Servo

1. Connect single servo (no others on bus) → scan-range should be 1-254
2. Assign proper ID:        --assign-id 1:5
3. Set angle limits:        --angle-limit 5:512:3584
4. Set default acc/speed:   --set-acc 5:50 --set-speed 5:300
5. Lock EEPROM:             --lock

6. Launch the Dashboard

cd stservo-sdk
python -m streamlit run examples/homing_dashboard.py

Dashboard Feature Roadmap

These features are planned additions to homing_dashboard.py to support higher-level application development.

Currently Implemented

• Port management + scan
• Full one-shot calibration (ID, limits, acc, speed, mode, torque, baud)
• Servo Inspector with single telemetry snapshot

Planned Features (Priority Order)

1. Real-Time Telemetry Streaming ⭐ HIGH

Purpose: Debug control loops; verify that position commands are followed.

• Poll joints 1–6 positions/speeds at configurable Hz (5–50 Hz)
• Render as live st.line_chart or Plotly time-series
• State: streaming: bool, poll_interval_ms, history_seconds
• Use st.empty() + time.sleep or st_autorefresh component
• Expose toggle: "Stream Position", "Stream Speed", "Stream Current"

2. Joint Zeroing Wizard ⭐ HIGH

Purpose: Step-by-step guided homing for soarm100 (IDs 1–6) into a specific physical zero pose.

• Multi-step form: select joints → torque-off → user moves arm to physical zero → read raw positions → compute STS_OFS_L offsets → confirm → write offsets → set limits → re-lock
• Show live position readout per joint during manual positioning
• Physical zero pose is a user-defined reference (not simply position 2048); save the target value per joint in session state
• Generate config snapshot JSON at end of wizard

3. Position Recorder & Replayer ⭐ HIGH

Purpose: Capture demonstration trajectories to bootstrap learning-based controllers.

• "Record" button polls ReadPosSpeed for IDs 1–6 at fixed Hz
• Saves timestamped CSV: timestamp, id, position, speed
• "Replay" button sends trajectory back via SyncWritePosEx + groupSyncWrite.txPacket()
• Controls: speed scale, loop count, delay between points

4. Position/Velocity Command Panel ⭐ HIGH

Purpose: Send direct position or velocity commands to one or all joints from the UI.

• Mode selector: "Servo mode" (position + speed + acc) or "Wheel mode" (continuous speed)
• Per-joint sliders: target position (0–4095), speed (0–3000), acc (0–254)
• "Send All" fires one groupSyncWrite.txPacket() for IDs 1–6 at once
• Live feedback: actual position overlaid on commanded target
• Captures the user's freeform request: "position/velocity command to move joints"

5. Sync Movement Tester ⭐ HIGH

Purpose: Validate synchronized multi-joint commands before integrating with the motion planner.

• Table UI: one row per joint with position / speed / acc sliders
• All joints commanded in a single sync packet

6. Health Monitor ⭐ HIGH

Purpose: Catch thermal, overload, or voltage errors during long runs.

• Background polling loop checks ReadStatus, ReadTemperature, ReadCurrent for IDs 1–6
• Raises st.warning / st.error badges when error bits are set
• Configurable thresholds: temp > 70°C, current > 1500 mA, etc.
• Log to in-memory deque for download

7. Configuration Export / Import ⭐ HIGH

Purpose: Reproducible robot setup across sessions and hardware.

• "Export" reads current register state of all detected servos → saves YAML to configs/
• "Import" loads a YAML file and applies it with a confirmation step
• Integrates with the existing configs/ folder in the main project

8. Offset Calibration Tool

Purpose: Fine-tune zero offsets without re-running full homing.

• Per-joint: show current ReadCorrection value, slider to adjust by ±200 counts
• Live position readout updates inline
• EEPROM unlock/lock handled transparently

9. Workspace Validation Sweep

Purpose: Verify that angle limits are physically correct.

• For each selected joint, sweep from MIN to MAX in N steps
• Record actual positions reached vs commanded; flag deviations > threshold
• Output: pass/fail per joint; suggest updated limits

Working with Upstream Modules

When a feature in fullstack_manip/ (e.g., core/robot.py, control/) needs servo-level access:

1. Import PortHandler and sts directly from stservo_sdk
2. Use GroupSyncRead / GroupSyncWrite for high-frequency joint state loops (read 4 bytes position+speed in one packet per servo)
3. For strict timing, call read4ByteTxRx / SyncWritePosEx + txPacket inside a LoopRateLimiter from fullstack_manip/utils/loop_rate_limiters.py
4. Never leave EEPROM unlocked during normal operation

Baud Rate Codes

Communication Result Codes