diff --git a/MKS SERVO42C User Manual V1.1.2.pdf b/MKS SERVO42C User Manual V1.1.2.pdf new file mode 100644 index 0000000..34754a2 Binary files /dev/null and b/MKS SERVO42C User Manual V1.1.2.pdf differ diff --git a/MKS42C.cpp b/MKS42C.cpp new file mode 100644 index 0000000..d829a64 --- /dev/null +++ b/MKS42C.cpp @@ -0,0 +1,203 @@ +#include "MKS42C.h" + +MKS42C::MKS42C(HardwareSerial& serial, uint8_t address) { + _serial = &serial; + _addr = address; + _isMoving = false; +} + +// Вспомогательная функция для CRC (стр. 13 PDF) +uint8_t MKS42C::calculateCRC(uint8_t* data, uint8_t len) { + uint16_t checksum = 0; + for (uint8_t i = 0; i < len; i++) { + checksum += data[i]; + } + return (uint8_t)(checksum & 0xFF); +} + +void MKS42C::sendRaw(uint8_t* data, uint8_t len) { + _serial->write(data, len); + _serial->write(calculateCRC(data, len)); +} + +void MKS42C::clearBuffer() { + while (_serial->available()) { + _serial->read(); + } +} + +// --- УПРАВЛЕНИЕ --- + +void MKS42C::setEnable(bool state) { + // Команда 0xF3 (стр. 16 PDF) + uint8_t cmd[] = {_addr, 0xF3, (uint8_t)(state ? 0x01 : 0x00)}; + sendRaw(cmd, 3); +} + +void MKS42C::run(uint8_t dir, uint8_t speed) { + // Команда 0xF6 (стр. 16 PDF) + // dir: 0 - CW, 1 - CCW + // speed: 0-127 + uint8_t val = ((dir & 0x01) << 7) | (speed & 0x7F); + uint8_t cmd[] = {_addr, 0xF6, val}; + + sendRaw(cmd, 3); + _isMoving = true; + _lastMoveTime = millis(); + _lastAbsPos = readAbsolutePosition(); +} + +void MKS42C::run(int16_t speed) { + uint8_t dir = (speed < 0) ? 1 : 0; // 1 - CCW, 0 - CW + uint8_t absSpeed = abs(speed); + + if (absSpeed > 127) absSpeed = 127; // Максимальная скорость 7 бит + + // Формируем байт: 7-й бит это направление, 0-6 биты это скорость + uint8_t val = (dir << 7) | (absSpeed & 0x7F); + + // Передаем пакет из 3 байт + CRC + uint8_t cmd[] = {_addr, 0xF6, val}; + sendRaw(cmd, 3); + + _isMoving = true; +} + +void MKS42C::stop() { + // Команда 0xF7 (стр. 16 PDF) + uint8_t cmd[] = {_addr, 0xF7}; + sendRaw(cmd, 2); + _isMoving = false; +} + +void MKS42C::rotateDegrees(float degrees, uint8_t speed) { + // Команда 0xFD (стр. 17 PDF) + // Используем проверенный коэффициент 3200 имп/оборот + long pulses = (long)((degrees / 360.0) * 3200.0); + uint8_t dir = (pulses >= 0) ? 0x00 : 0x01; + uint32_t absP = abs(pulses); + + uint8_t cmd[] = { + _addr, + 0xFD, + (uint8_t)((dir << 7) | (speed & 0x7F)), + (uint8_t)((absP >> 24) & 0xFF), + (uint8_t)((absP >> 16) & 0xFF), + (uint8_t)((absP >> 8) & 0xFF), + (uint8_t)(absP & 0xFF) + }; + sendRaw(cmd, 7); + _isMoving = true; + _lastMoveTime = millis(); + _lastAbsPos = readAbsolutePosition(); +} + +// --- ЧТЕНИЕ ДАННЫХ (8-байтовый пакет из V1.1.2) --- + +float MKS42C::readPosition() { + // 1. Убираем clearBuffer() отсюда — он может "съедать" начало ответа + + // 2. Шлем запрос + uint8_t cmd[] = {_addr, 0x30}; + sendRaw(cmd, 2); + delay(5); + // 3. Ждем появления данных (увеличим до 200мс) + unsigned long start = millis(); + while (_serial->available() < 8 && millis() - start < 200) { + delay(1); + } + + if (_serial->available() < 8) { + // Если данных нет, попробуем еще раз послать запрос (иногда первый теряется) + sendRaw(cmd, 2); + start = millis(); + while (_serial->available() < 8 && millis() - start < 200) delay(1); + } + + if (_serial->available() >= 8) { + // 4. Ищем заголовок 0xE0 + while (_serial->available() > 0 && _serial->peek() != _addr) { + _serial->read(); // Выкидываем мусор + } + + if (_serial->available() >= 8) { + uint8_t buf[8]; + _serial->readBytes(buf, 8); + + // Считаем CRC + uint16_t sum = 0; + for (int i = 0; i < 7; i++) sum += buf[i]; + + if ((uint8_t)(sum & 0xFF) == buf[7]) { + uint16_t value = ((uint16_t)buf[5] << 8) | buf[6]; + return (value * 360.0) / 65536.0; + } else { + Serial.print("CRC Err: "); Serial.println((uint8_t)(sum & 0xFF), HEX); + } + } + } else { + Serial.print("Buf low: "); Serial.println(_serial->available()); + } + return -1.0; +} + +long MKS42C::readAbsolutePosition() { + static long lastValidPos = 0; // Храним последнее успешное значение + + uint8_t cmd[] = {_addr, 0x30}; + sendRaw(cmd, 2); + + delay(5); + + unsigned long start = millis(); + while (_serial->available() < 8 && millis() - start < 100); + + if (_serial->available() >= 8) { + while (_serial->available() > 8 && _serial->peek() != _addr) _serial->read(); + + uint8_t buf[8]; + _serial->readBytes(buf, 8); + + uint16_t sum = 0; + for (int i = 0; i < 7; i++) sum += buf[i]; + + if ((uint8_t)(sum & 0xFF) == buf[7]) { + int32_t carry = ((int32_t)(int8_t)buf[1] << 24) | + ((uint32_t)buf[2] << 16) | + ((uint32_t)buf[3] << 8) | + (uint32_t)buf[4]; + uint16_t value = ((uint16_t)buf[5] << 8) | buf[6]; + lastValidPos = (long)((long long)carry * 65536LL + value); + } + } + return lastValidPos; // Возвращаем последнее нормальное, если текущее битое +} + +bool MKS42C::isBusy() { + while (_serial->available() >= 3) { + if (_serial->peek() != _addr) { _serial->read(); continue; } + uint8_t b[3]; + _serial->readBytes(b, 3); + if (b[1] == 0x02) { + _isMoving = false; + return false; + } + } + + if (_isMoving) { + long currentPos = readAbsolutePosition(); + + // Если позиция изменилась — мотор точно едет + if (currentPos != _lastAbsPos) { + _lastAbsPos = currentPos; + _lastMoveTime = millis(); + return true; + } + + if (millis() - _lastMoveTime > 150) { + _isMoving = false; + } + } + + return _isMoving; +} \ No newline at end of file diff --git a/MKS42C.h b/MKS42C.h new file mode 100644 index 0000000..a656fd4 --- /dev/null +++ b/MKS42C.h @@ -0,0 +1,77 @@ +#ifndef MKS42C_H +#define MKS42C_H + +#include + +class MKS42C { +public: + /** + * @brief Конструктор + * @param serial Ссылка на HardwareSerial (например, Serial2) + * @param address Адрес мотора (по умолчанию 0xE0) + */ + MKS42C(HardwareSerial& serial, uint8_t address = 0xE0); + + // --- Управление питанием --- + void setEnable(bool state); // Включить/выключить удержание вала + + // --- Движение --- + /** + * @brief Непрерывное вращение (Команда 0xF6) + * @param dir Направление: 0 - CW (по часовой), 1 - CCW (против) + * @param speed Скорость (0-127) + */ + void run(uint8_t dir, uint8_t speed); + + + /** + * @brief Непрерывное вращение + * @param speed Скорость от -127 до 127 (минус меняет направление) + */ + void run(int16_t speed); + + /** + * @brief Поворот на заданный угол (Команда 0xFD) + * @param degrees Угол в градусах (положительный - CW, отрицательный - CCW) + * @param speed Скорость (0-127) + */ + void rotateDegrees(float degrees, uint8_t speed); + + /** + * @brief Мгновенная остановка (Команда 0xF7) + */ + void stop(); + + // --- Чтение данных (Команда 0x30) --- + /** + * @brief Чтение текущего угла в пределах одного круга + * @return Угол от 0.0 до 359.99 или -1.0 при ошибке + */ + float readPosition(); + + /** + * @brief Чтение абсолютной позиции (с учетом оборотов) + * @return Общее количество импульсов (1 оборот = 65536 единиц энкодера) + */ + long readAbsolutePosition(); + + /** + * @brief Проверка, движется ли мотор в данный момент + * @return true если занят, false если остановился + */ + bool isBusy(); + +private: + HardwareSerial* _serial; + uint8_t _addr; + long _lastAbsPos = 0; + unsigned long _lastMoveTime = 0; + bool _isMoving; + + // Вспомогательные функции + uint8_t calculateCRC(uint8_t* data, uint8_t len); + void sendRaw(uint8_t* data, uint8_t len); + void clearBuffer(); +}; + +#endif \ No newline at end of file diff --git a/README.md b/README.md index 98ee7dc..af775a5 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,103 @@ -# MKS42C +# MKS SERVO42C Arduino Library +Легкая библиотека для управления умными шаговыми моторами MKS SERVO42C через интерфейс RS485/UART. Поддерживает чтение высокоточного энкодера (16 бит) и управление движением в реальном времени. +🚀 Основные возможности + + Движение: Поворот на заданный угол, бесконечное вращение, мгновенная остановка. + + Обратная связь: Чтение текущего угла (0-360°) и абсолютной позиции (с учетом полных оборотов). + + Умный статус: Отслеживание завершения движения (isBusy) через анализ данных энкодера. + +🛠 Описание функций (API) +Конструктор + +MKS42C(HardwareSerial& serial, uint8_t address = 0xE0); + + serial: Ссылка на аппаратный Serial (например, Serial2 для ESP32). + + address: CAN/RS485 адрес мотора (по умолчанию 0xE0). + +Управление движением +void setEnable(bool state); + +Включает (true) или выключает (false) ток в обмотках. При false вал можно вращать рукой, при true мотор удерживает позицию. +void run(int16_t speed); + +Самый удобный способ вращения. * Передавай положительное значение (например, 40) для вращения по часовой стрелке. + + Передавай отрицательное значение (например, -40) для вращения против часовой стрелки. + + Диапазон скорости: от -127 до 127. + +void rotateDegrees(float degrees, uint8_t speed); + +Поворот на точный угол относительно текущей позиции. + + degrees: Угол (например, 90.0 или -720.0). + + speed: Скорость движения (1-127). + +void stop(); + +Мгновенная программная остановка мотора. +Чтение данных +float readPosition(); + +Возвращает текущий угол вала в диапазоне 0.00° ... 359.99°. + + Если возвращает -1.0, значит возникла ошибка связи (CRC или таймаут). + +long readAbsolutePosition(); + +Возвращает общее количество импульсов энкодера с момента включения. + + 1 оборот = 65536 единиц. + + Значение может быть отрицательным. Это лучший способ отслеживать пройденный путь. + +bool isBusy(); + +Проверяет, движется ли мотор. + + Библиотека анализирует пакеты статуса от мотора и изменение данных энкодера. + + Возвращает true, если вал еще вращается. + +📐 Формулы для ручного управления + +Если ты хочешь реализовать свою логику в основном коде: + + Градусы в импульсы: Pulses=360Degrees×65536​ + + Импульсы в градусы: Degrees=65536Pulses×360​ + +📋 Пример: Возврат в абсолютный ноль + +Этот код заставляет мотор крутиться в сторону «нулевой» точки, пока он ее не достигнет. +C++ + +void backToZero() { + long currentPos = motor.readAbsolutePosition(); + + // Выбираем скорость: если мы в плюсе, едем назад (-40), если в минусе — вперед (40) + int16_t speed = (currentPos > 0) ? -40 : 40; + motor.run(speed); + + while (true) { + currentPos = motor.readAbsolutePosition(); + // Останавливаемся, когда пересекли ноль или подошли очень близко + if (speed < 0 && currentPos <= 10) break; + if (speed > 0 && currentPos >= -10) break; + delay(10); + } + motor.stop(); +} + +⚠️ Важные замечания + + Общая земля: Обязательно соедини GND контроллера и GND драйвера мотора. + + Скорость UART: Убедись, что в коде Serial.begin(38400) и в настройках на экране мотора скорость совпадает. + + Резисторы: Для длинных линий UART (более 1 метра) рекомендуется использовать подтягивающие резисторы или модули RS485 \ No newline at end of file diff --git a/examples/MKS42C_Example/MKS42C_Example.ino b/examples/MKS42C_Example/MKS42C_Example.ino new file mode 100644 index 0000000..54079ec --- /dev/null +++ b/examples/MKS42C_Example/MKS42C_Example.ino @@ -0,0 +1,71 @@ +#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(" > Ошибка чтения данных..."); + } +} \ No newline at end of file diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..d33988b --- /dev/null +++ b/keywords.txt @@ -0,0 +1,11 @@ +MKS42C KEYWORD1 +setEnable KEYWORD2 +stop KEYWORD2 +setHome KEYWORD2 +setTorque KEYWORD2 +rotateDegrees KEYWORD2 +run KEYWORD2 +readAbsolutePosition KEYWORD2 +readPosition KEYWORD2 +isBusy KEYWORD2 +isBlocked KEYWORD2 \ No newline at end of file diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..03669bb --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=MKS42C +version=1.0 +author=ArturKarasevich +maintainer=ArturKarasevich +sentence=MKS42C Control +paragraph=I didnt come up with it +category=Category +url=https://git.chepuhagram.ru/ArturKarasevich/MKS42C.git +architectures=esp32 diff --git a/scr/MKS42C.cpp b/scr/MKS42C.cpp deleted file mode 100644 index 813a2c8..0000000 --- a/scr/MKS42C.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include "MKS42C.h" - -MKS42C::MKS42C(HardwareSerial& serial, uint8_t address) { - _serial = &serial; - _addr = address; - _isMoving = false; -} - -uint8_t MKS42C::calculateCRC(uint8_t* data, uint8_t len) { - // вычисление контрольной суммы - uint16_t checksum = 0; - for (uint8_t i = 0; i < len; i++) { - checksum += data[i]; - } - return (uint8_t)(checksum & 0xFF); -} - -void MKS42C::sendRaw(uint8_t* data, uint8_t len) { - // отправить в порт - _serial->write(data, len); - _serial->write(calculateCRC(data, len)); -} - -void MKS42C::clearBuffer() { - // очистить входящий буфер - while (_serial->available()) { - _serial->read(); - } -} - -void MKS42C::setEnable(bool state) { - // вкл / выкл движка - uint8_t cmd[] = {_addr, 0xF3, (uint8_t)(state ? 0x01 : 0x00)}; - sendRaw(cmd, 3); -} - -void MKS42C::stop() { - // стоп - uint8_t cmd[] = {_addr, 0xF7}; - sendRaw(cmd, 2); - _isMoving = false; -} - -void MKS42C::setHome() { - // задать текущую позицию, как 0 (сброс энкодера) - uint8_t cmd[] = {_addr, 0x34}; - sendRaw(cmd, 2); - delay(100); -} - -void MKS42C::setTorque(uint16_t ma) { - // задать момент вращения - uint8_t cmd[] = {_addr, 0xAF, (uint8_t)((ma >> 8) & 0xFF), (uint8_t)(ma & 0xFF)}; - sendRaw(cmd, 4); -} - -void MKS42C::run(uint8_t dir, uint8_t speed) { - // просто включить - uint8_t val = ((dir & 0x01) << 7) | (speed & 0x7F); - uint8_t cmd[] = {_addr, 0xF6, val}; - - sendRaw(cmd, 3); - _isMoving = true; -} - -void MKS42C::rotateDegrees(float degrees, uint8_t speed) { - // включить на определённое количество гражут=сов - long pulses = (long)((degrees / 360.0) * 16384.0); - uint8_t dir = (pulses >= 0) ? 0x00 : 0x01; - uint32_t absP = abs(pulses); - - uint8_t cmd[] = { - _addr, - 0xFD, - (uint8_t)((dir << 7) | (speed & 0x7F)), - (uint8_t)((absP >> 24) & 0xFF), - (uint8_t)((absP >> 16) & 0xFF), - (uint8_t)((absP >> 8) & 0xFF), - (uint8_t)(absP & 0xFF) - }; - sendRaw(cmd, 7); - _isMoving = true; -} - -float MKS42C::readPosition() { - // считать значение энкодера - clearBuffer(); - uint8_t cmd[] = {_addr, 0x30}; - sendRaw(cmd, 2); - - unsigned long start = millis(); - while (_serial->available() < 5 && millis() - start < 50); - - if (_serial->available() >= 5) { - uint8_t buf[5]; - _serial->readBytes(buf, 5); - - uint16_t rawPos = (uint16_t)((buf[2] << 8) | buf[3]); - - // Переводим в понятные градусы - return (rawPos * 360.0) / 16384.0; - } - - return -1.0; -} - -long MKS42C::readAbsolutePosition() { - // считать абсолютное значение энкодера - clearBuffer(); - uint8_t cmd[] = {_addr, 0x31}; - sendRaw(cmd, 2); - - unsigned long start = millis(); - while (_serial->available() < 6 && millis() - start < 100); - - if (_serial->available() >= 6) { - uint8_t buf[6]; - _serial->readBytes(buf, 6); - uint32_t rawPos = ((uint32_t)buf[2] << 24) | ((uint32_t)buf[3] << 16) | - ((uint32_t)buf[4] << 8) | (uint32_t)buf[5]; - return (buf[1] == 0) ? (long)rawPos : -(long)rawPos; - } - return 0; -} - -bool MKS42C::isBusy() { - // проверить движется ли мотор - if (_serial->available() >= 3) { - uint8_t buf[3]; - if (_serial->peek() == _addr) { - _serial->readBytes(buf, 3); - if (buf[1] == 0x02) { - _isMoving = false; - return false; - } - } - } - return _isMoving; -} - -bool MKS42C::isBlocked() { - // проверить блокировку вала - clearBuffer(); - uint8_t cmd[] = {_addr, 0x3A}; - sendRaw(cmd, 2); - - unsigned long start = millis(); - while (_serial->available() < 3 && millis() - start < 50); - - if (_serial->available() >= 3) { - uint8_t buf[3]; - _serial->readBytes(buf, 3); - return (buf[1] == 0x02); - } - return false; -} \ No newline at end of file diff --git a/scr/MKS42C.h b/scr/MKS42C.h deleted file mode 100644 index 675b48b..0000000 --- a/scr/MKS42C.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef MKS42C_H -#define MKS42C_H - -#include -#include - -class MKS42C { -public: - /** - * @brief Конструктор класса - * @param serial Ссылка на объект HardwareSerial (например, Serial2) - * @param address UART адрес мотора (по умолчанию 0xE0) - */ - MKS42C(HardwareSerial& serial, uint8_t address = 0xE0); - - /** - * @brief Включить или выключить удержание вала - * @param state true - включить (Enable), false - отключить (Disable) - */ - void setEnable(bool state); - - /** - * @brief Остановка двигателя - */ - void stop(); - - /** - * @brief Установить текущую позицию как нулевую (Home) - * Данные записываются в EEPROM драйвера. - */ - void setHome(); - - /** - * @brief Установка крутящего момента через ограничение тока - * @param ma Ток в миллиамперах (рекомендуется 0 - 2000) - */ - void setTorque(uint16_t ma); - - /** - * @brief Запустить непрерывное вращение - * @param dir Направление: 0 - CW (по часовой), 1 - CCW (против часовой) - * @param speed Скорость вращения (0 - 255) - */ - void run(uint8_t dir, uint8_t speed); - - /** - * @brief Поворот на определенное количество градусов - * @param degrees Угол (положительный или отрицательный) - * @param speed Скорость вращения (0 - 255) - */ - void rotateDegrees(float degrees, uint8_t speed); - - /** - * @brief Чтение текущей позиции вала (0-360 градусов) - * @return Позиция в градусах (float) - */ - float readPosition(); - - /** - * @brief Чтение абсолютной позиции энкодера (с учетом полных оборотов) - * @return Количество импульсов (14-бит энкодер: 16384 на оборот при MStep 16) - */ - long readAbsolutePosition(); - - /** - * @brief Проверка, завершил ли мотор движение (по команде rotateDegrees) - * @return true - мотор еще в пути, false - мотор остановился - */ - bool isBusy(); - - /** - * @brief Проверка состояния защиты (блокировка вала) - * @return true - сработала защита (Protect), false - нормальная работа - */ - bool isBlocked(); - -private: - HardwareSerial* _serial; - uint8_t _addr; - bool _moving; - - uint8_t calculateCRC(uint8_t* data, uint8_t len); - void sendRaw(uint8_t* data, uint8_t len); - void clearBuffer(); -}; - -#endif \ No newline at end of file