/*
 * STS3215 — dual-servo gate motion test for the XIAO vending machine.
 * Both servos sweep HOME (1024) → TRIGGER (0) → HOME twice, matching
 * the dispense cycle used in the group firmware.
 *
 * 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_NUM 2
byte ID[SERVO_NUM]    = {1, 2};
u16  Speed[SERVO_NUM] = {1500, 1500};
byte ACC[SERVO_NUM]   = {50, 50};
s16  Pos[SERVO_NUM]   = {1024, 1024};

const s16 HOME_POS    = 1024;
const s16 TRIGGER_POS = 0;
const byte CYCLES       = 2;
const unsigned long FORWARD_MS = 1800;
const unsigned long RETURN_MS  = 1200;

void moveBothTo(s16 target) {
  for (int i = 0; i < SERVO_NUM; i++) Pos[i] = target;
  st.SyncWritePosEx(ID, SERVO_NUM, Pos, Speed, ACC);
}

void runGateCycles(byte cycles) {
  for (byte c = 0; c < cycles; c++) {
    Serial.println("Gate OPEN");
    moveBothTo(TRIGGER_POS);
    delay(FORWARD_MS);
    Serial.println("Gate CLOSE");
    moveBothTo(HOME_POS);
    delay(RETURN_MS);
  }
}

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

  Serial.println("Dual STS3215 gate test — press any key in Serial Monitor to run");
}

void loop() {
  if (Serial.available()) {
    while (Serial.available()) Serial.read();
    runGateCycles(CYCLES);
    Serial.println("Cycle complete");
  }
}