/*
 * STS3215 — assign a unique bus ID to one servo at a time.
 * Flash this sketch, connect ONLY the target servo to the bus,
 * then open Serial Monitor at 115200 baud and follow the prompts.
 *
 * Library: Waveshare SCServo (SMS_STS class)
 * Board:   Seeed Wio Terminal — servos on Grove UART (Serial1)
 */
#include "SCServo.h"

SMS_STS st;

#define SERVO_RX PIN_WIRE_SCL   // Grove UART RX on Wio Terminal
#define SERVO_TX PIN_WIRE_SDA   // Grove UART TX on Wio Terminal

void setup() {
  Serial.begin(115200);
  while (!Serial) {}

  Serial1.begin(1000000, SERIAL_8N1, SERVO_RX, SERVO_TX);
  st.pSerial = &Serial1;

  Serial.println("STS3215 ID assignment");
  Serial.println("Connect ONE servo only, then enter the new ID (1-253):");
}

void loop() {
  if (Serial.available()) {
    int newId = Serial.parseInt();
    if (newId < 1 || newId > 253) {
      Serial.println("Invalid ID — use 1-253");
      return;
    }
    if (st.Ping(254) != 254) {
      Serial.println("No servo detected on bus");
      return;
    }
    if (st.unLockEprom(254) == 1 && st.writeByte(254, SMS_STS_ID, newId) == 1) {
      st.LockEprom(newId);
      Serial.print("ID set to ");
      Serial.println(newId);
    } else {
      Serial.println("Write failed — check wiring and power");
    }
  }
}