73 lines
2.2 KiB
C++
73 lines
2.2 KiB
C++
#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(" > Ошибка чтения данных...");
|
||
}
|
||
} |