#include "MKS42C.h"

// Используем Serial2 (Пины ESP32: TX=17, RX=16)
// Скорость должна совпадать с настройками в меню мотора!
MKS42C motor(Serial2, 0xE0); 

void setup() {
  Serial.begin(115200);
  Serial2.begin(115200, SERIAL_8N1, 16, 17); // Или 115200, если поменял в меню
  
  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); // 0 - по часовой
  
  unsigned long startTime = millis();
  while(millis() - startTime < 5000) { // Крутим 5 секунд
    printStatus();
    delay(200);
  }

  // 4. Тест stop
  Serial.println("Действие: Остановка!");
  motor.stop();
  delay(1000);
  
  Serial.println("Тест окончен. Текущая финальная позиция:");
  printStatus();
}

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

// Вспомогательная функция для красивого вывода
void printStatus() {
  float angle = motor.readPosition();
  long absPos = motor.readAbsolutePosition();
  
  if (angle >= 0) { // Если чтение успешно
    Serial.print(" > Угол: ");
    Serial.print(angle);
    Serial.print("° | Абс. импульсы: ");
    Serial.println(absPos);
  } else {
    Serial.println(" > Ошибка чтения данных...");
  }
}