04-04-26-23-51
This commit is contained in:
parent
cce4c3d5b7
commit
735c604ffb
Binary file not shown.
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,77 @@
|
||||||
|
#ifndef MKS42C_H
|
||||||
|
#define MKS42C_H
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
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
|
||||||
103
README.md
103
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
|
||||||
|
|
@ -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(" > Ошибка чтения данных...");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
156
scr/MKS42C.cpp
156
scr/MKS42C.cpp
|
|
@ -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;
|
|
||||||
}
|
|
||||||
87
scr/MKS42C.h
87
scr/MKS42C.h
|
|
@ -1,87 +0,0 @@
|
||||||
#ifndef MKS42C_H
|
|
||||||
#define MKS42C_H
|
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include <HardwareSerial.h>
|
|
||||||
|
|
||||||
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
|
|
||||||
Loading…
Reference in New Issue