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