#include "MKS42C.h"

// Используем Serial2 (Пины ESP32: TX=17, RX=16)
MKS42C motor(Serial2, 0xE0);

void setup() {
  Serial.begin(115200);
  // Инициализация Serial2 для общения с мотором
  Serial2.begin(38400, SERIAL_8N1, 16, 17);

  delay(2000);
  Serial.println("=== Тестирование MKS SERVO42C V1.1.2 ===");

  // 1. Включаем удержание вала
  motor.setEnable(true);
  delay(500);

  // 2. Тест rotateDegrees (Поворот на 180 градусов)
  Serial.println("Действие: Поворот на 180°");
  motor.rotateDegrees(180, 60);

  // Ждем завершения движения
  while (motor.isBusy()) {
    printStatus();
    delay(100);
  }
  Serial.println("Движение 180° завершено.");
  delay(2000);

  // 3. Тест run (Непрерывное вращение)
  Serial.println("Действие: Запуск постоянного вращения (Скорость 40)");
  motor.run(0, 40);

  unsigned long startTime = millis();
  while (millis() - startTime < 3000) { // Покрутим 3 секунды
    printStatus();
    delay(200);
  }

  // 4. Остановка перед возвратом
  Serial.println("Действие: Остановка!");
  motor.stop();
  delay(1500);
  printStatus();

  // Отключаем удержание, если нужно расслабить вал
  motor.setEnable(false);
}

void loop() {
  // Мониторинг позиции (если крутить вал рукой, увидим смещение от 0)
  static unsigned long lastPrint = 0;
  if (millis() - lastPrint > 500) {
    printStatus();
    lastPrint = millis();
  }
}

void printStatus() {
  float angle = motor.readPosition();
  // Даем небольшую паузу между запросами для стабильности UART
  delay(10);
  long absPos = motor.readAbsolutePosition();

  if (angle >= 0) {
    Serial.print(" > Угол: ");
    Serial.print(angle);
    Serial.print("° | Абс. импульсы: ");
    Serial.println(absPos);
  } else {
    Serial.println(" > Ошибка чтения данных...");
  }
}