/*
 * STS3215 — single-servo position sweep demo.
 * Moves one servo through three positions to verify bus communication
 * and position feedback before integrating into the vending machine.
 *
 * 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
#define SERVO_TX PIN_WIRE_SDA
#define SERVO_ID 1

const s16 POSITIONS[] = {0, 1024, 2048, 1024, 0};
const byte NUM_POSITIONS = sizeof(POSITIONS) / sizeof(POSITIONS[0]);
const u16 SPEED = 1500;
const byte ACC  = 50;

void setup() {
  Serial.begin(115200);
  Serial1.begin(1000000, SERIAL_8N1, SERVO_RX, SERVO_TX);
  st.pSerial = &Serial1;
  delay(1000);

  Serial.print("Ping ID ");
  Serial.print(SERVO_ID);
  Serial.print(" → ");
  Serial.println(st.Ping(SERVO_ID));
}

void loop() {
  for (byte i = 0; i < NUM_POSITIONS; i++) {
    Serial.print("Move to ");
    Serial.println(POSITIONS[i]);
    st.WritePosEx(SERVO_ID, POSITIONS[i], SPEED, ACC);
    delay(2000);

    s16 pos = st.ReadPos(SERVO_ID);
    Serial.print("Feedback position: ");
    Serial.println(pos);
  }
}