# our_feetech_controller

Feetech STS servo utilities. All scripts use `scservo_sdk` (installed via `pip install feetech-servo-sdk`) with a local `scservo_patch.py` that monkey-patches the SDK for API compatibility (auto-injects portHandler into all read/write/ping calls).

Default connection: `/dev/tty.usbserial-0001` at 115200 baud. Both are hardcoded as constants at the top of each script.

## Scripts

### feetech_read_motor.py

Live monitor for one or more Feetech servos. Prints position, speed, load, voltage, temperature, current, and moving status at ~20Hz, updating in-place.

```
python feetech_read_motor.py <motor_id> [motor_id ...]
```

Reads the following registers per motor:
- Position (addr 56-57, via `ReadPos`)
- Speed (addr 58-59, via `ReadSpeed`, sign-extended)
- Load (addr 60-61, sign-extended)
- Voltage (addr 62, units of 0.1V)
- Temperature (addr 63, degrees C)
- Current (addr 69-70, units of 6.5mA)
- Moving (addr 66, via `ReadMoving`)

### calibrate_motors.py

Sets the current physical position of each specified motor as its middle position (2048) using the `INST_OFSCAL` (0x0B) instruction. This writes to the servo's EPROM offset register (addr 31-32).

```
python calibrate_motors.py <motor_id> [motor_id ...]
```

Workflow:
1. Physically move each motor to the desired center position
2. Run the script with the motor IDs
3. Confirms before writing
4. Sends `INST_OFSCAL` to each motor, which stores the offset so the current position reads as 2048

### set_motor_id.py

Changes a servo's ID by writing to the EPROM ID register (addr 5). Unlocks EPROM (addr 55 = 0), writes the new ID, then relocks (addr 55 = 1) using the new ID.

```
python set_motor_id.py <old_id> <new_id>
```

Only one servo should be on the bus when running this, otherwise every servo responding to `old_id` will be changed.

Quirk: when writing the new ID, the servo changes its ID before sending the ack, so the reply comes back tagged with the new ID and the SDK reports `COMM_RX_TIMEOUT (-6)`. The script ignores that return code on the ID-write step and verifies success by pinging the new ID.

## Key Feetech STS notes

- Motor IDs: the servos on our bus start at ID 0
- Position range: 0-4096 (12-bit), 2048 = middle
- No hardware direction-reversal register exists; direction must be handled in software with a signs array (e.g. `signs = [-1, 1, 1, 1, 1, 1]`)
- `scservo_patch.py` is required because the installed SDK's `sms_sts` class expects `portHandler` as the first arg to every method; the patch wraps these so you only pass `(motor_id, ...)`
- Common error codes: `COMM_RX_TIMEOUT = -6` usually means wrong baud rate or motor not connected