156 lines
4.1 KiB
C++
156 lines
4.1 KiB
C++
#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;
|
||
} |