first
This commit is contained in:
5
.gitignore
vendored
Normal file
5
.gitignore
vendored
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
.pio
|
||||||
|
.vscode/.browse.c_cpp.db*
|
||||||
|
.vscode/c_cpp_properties.json
|
||||||
|
.vscode/launch.json
|
||||||
|
.vscode/ipch
|
||||||
10
.vscode/extensions.json
vendored
Normal file
10
.vscode/extensions.json
vendored
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
{
|
||||||
|
// See http://go.microsoft.com/fwlink/?LinkId=827846
|
||||||
|
// for the documentation about the extensions.json format
|
||||||
|
"recommendations": [
|
||||||
|
"platformio.platformio-ide"
|
||||||
|
],
|
||||||
|
"unwantedRecommendations": [
|
||||||
|
"ms-vscode.cpptools-extension-pack"
|
||||||
|
]
|
||||||
|
}
|
||||||
60
.vscode/settings.json
vendored
Normal file
60
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,60 @@
|
|||||||
|
{
|
||||||
|
"window.zoomLevel": 0,
|
||||||
|
"C_Cpp_Runner.msvcBatchPath": "",
|
||||||
|
"C_Cpp_Runner.cCompilerPath": "gcc",
|
||||||
|
"C_Cpp_Runner.cppCompilerPath": "g++",
|
||||||
|
"C_Cpp_Runner.debuggerPath": "gdb",
|
||||||
|
"C_Cpp_Runner.cStandard": "",
|
||||||
|
"C_Cpp_Runner.cppStandard": "",
|
||||||
|
"C_Cpp_Runner.useMsvc": false,
|
||||||
|
"C_Cpp_Runner.warnings": [
|
||||||
|
"-Wall",
|
||||||
|
"-Wextra",
|
||||||
|
"-Wpedantic",
|
||||||
|
"-Wshadow",
|
||||||
|
"-Wformat=2",
|
||||||
|
"-Wcast-align",
|
||||||
|
"-Wconversion",
|
||||||
|
"-Wsign-conversion",
|
||||||
|
"-Wnull-dereference"
|
||||||
|
],
|
||||||
|
"C_Cpp_Runner.msvcWarnings": [
|
||||||
|
"/W4",
|
||||||
|
"/permissive-",
|
||||||
|
"/w14242",
|
||||||
|
"/w14287",
|
||||||
|
"/w14296",
|
||||||
|
"/w14311",
|
||||||
|
"/w14826",
|
||||||
|
"/w44062",
|
||||||
|
"/w44242",
|
||||||
|
"/w14905",
|
||||||
|
"/w14906",
|
||||||
|
"/w14263",
|
||||||
|
"/w44265",
|
||||||
|
"/w14928"
|
||||||
|
],
|
||||||
|
"C_Cpp_Runner.enableWarnings": true,
|
||||||
|
"C_Cpp_Runner.warningsAsError": false,
|
||||||
|
"C_Cpp_Runner.compilerArgs": [],
|
||||||
|
"C_Cpp_Runner.linkerArgs": [],
|
||||||
|
"C_Cpp_Runner.includePaths": [],
|
||||||
|
"C_Cpp_Runner.includeSearch": [
|
||||||
|
"*",
|
||||||
|
"**/*"
|
||||||
|
],
|
||||||
|
"C_Cpp_Runner.excludeSearch": [
|
||||||
|
"**/build",
|
||||||
|
"**/build/**",
|
||||||
|
"**/.*",
|
||||||
|
"**/.*/**",
|
||||||
|
"**/.vscode",
|
||||||
|
"**/.vscode/**"
|
||||||
|
],
|
||||||
|
"C_Cpp_Runner.useAddressSanitizer": false,
|
||||||
|
"C_Cpp_Runner.useUndefinedSanitizer": false,
|
||||||
|
"C_Cpp_Runner.useLeakSanitizer": false,
|
||||||
|
"C_Cpp_Runner.showCompilationTime": false,
|
||||||
|
"C_Cpp_Runner.useLinkTimeOptimization": false,
|
||||||
|
"C_Cpp_Runner.msvcSecureNoWarnings": false
|
||||||
|
}
|
||||||
357
VinceMotorControllerUsb.cpp
Normal file
357
VinceMotorControllerUsb.cpp
Normal file
@ -0,0 +1,357 @@
|
|||||||
|
#include "VinceMotorControllerUsb.h"
|
||||||
|
|
||||||
|
VinceMotorControllerUsb::VinceMotorControllerUsb(QString serialPortNumber, QString paramJson, QObject* parent)
|
||||||
|
:QObject(parent)
|
||||||
|
{
|
||||||
|
m_pSerialPort = new QSerialPort(this);
|
||||||
|
|
||||||
|
m_pSerialPort->setPortName(serialPortNumber);
|
||||||
|
m_pSerialPort->setReadBufferSize(512);
|
||||||
|
|
||||||
|
bool bRes = m_pSerialPort->setBaudRate(9600);
|
||||||
|
if (!bRes)
|
||||||
|
{
|
||||||
|
//qDebug() << "Err:setBaudRate Failed.Exit Code:1";
|
||||||
|
//std::cout << "Err.setBaudRate Failed" << std::endl;
|
||||||
|
printf("Err:setBaudRate Failed.Exit Code:1");
|
||||||
|
}
|
||||||
|
|
||||||
|
bRes = m_pSerialPort->open(QIODevice::ReadWrite);
|
||||||
|
if (!bRes)
|
||||||
|
{
|
||||||
|
//qDebug() << "Err:open Failed.Exit Code:2";
|
||||||
|
//std::cout << "Err.open Failed" << std::endl;
|
||||||
|
printf("Err:open Failed.Exit Code:2");
|
||||||
|
}
|
||||||
|
|
||||||
|
m_iSpeed = 3000;
|
||||||
|
|
||||||
|
init(paramJson);
|
||||||
|
}
|
||||||
|
|
||||||
|
VinceMotorControllerUsb::~VinceMotorControllerUsb()
|
||||||
|
{
|
||||||
|
delete m_pSerialPort;
|
||||||
|
}
|
||||||
|
|
||||||
|
int VinceMotorControllerUsb::sendCommand2Motor(QString cmd)
|
||||||
|
{
|
||||||
|
QByteArray commandData = cmd.toUtf8(); // <20><> QString ת<><D7AA>Ϊ QByteArray
|
||||||
|
qint64 bytesWritten = m_pSerialPort->write(commandData);
|
||||||
|
|
||||||
|
/*if (bytesWritten == -1) {
|
||||||
|
qDebug() << "д<><D0B4>ʧ<EFBFBD><CAA7>:" << m_pSerialPort->errorString();
|
||||||
|
}
|
||||||
|
else if (bytesWritten != commandData.size()) {
|
||||||
|
qDebug() << "д<>벻<EFBFBD><EBB2BB><EFBFBD><EFBFBD>";
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
qDebug() << "<22><><EFBFBD><EFBFBD>ͳɹ<CDB3>";
|
||||||
|
}*/
|
||||||
|
|
||||||
|
return bytesWritten;
|
||||||
|
}
|
||||||
|
|
||||||
|
int VinceMotorControllerUsb::recvData(QByteArray& dataRecv)
|
||||||
|
{
|
||||||
|
dataRecv.clear();
|
||||||
|
|
||||||
|
QByteArray temp;
|
||||||
|
|
||||||
|
temp = m_pSerialPort->readAll();
|
||||||
|
dataRecv.append(temp);
|
||||||
|
|
||||||
|
while (dataRecv.size() < 21)
|
||||||
|
{
|
||||||
|
m_pSerialPort->waitForReadyRead(100);
|
||||||
|
temp = m_pSerialPort->readAll();
|
||||||
|
dataRecv.append(temp);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
IrisMotorErrorCode VinceMotorControllerUsb::extractOneValidFrame(QByteArray& buffer)
|
||||||
|
{
|
||||||
|
//std::cout << "before------" << buffer.size() << std::endl;
|
||||||
|
//std::cout << buffer.toHex().toStdString() << std::endl;
|
||||||
|
|
||||||
|
IrisMotorErrorCode errorCode = MOTOR_NO_ERROR;
|
||||||
|
|
||||||
|
//<2F><>ʽ1<CABD><31>
|
||||||
|
/*int startPos = buffer.lastIndexOf('\xFF');
|
||||||
|
int endPos = buffer.indexOf('\xFE');
|
||||||
|
|
||||||
|
if (endPos <= startPos)
|
||||||
|
{
|
||||||
|
errorCode = MOTOR_INVALID_FRAME;
|
||||||
|
return errorCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
buffer = buffer.mid(startPos, endPos - startPos + 1);*/
|
||||||
|
|
||||||
|
//<2F><>ʽ2<CABD><32>
|
||||||
|
//ff00ffffff000200000000000000391f6900000d42030001feff00ff000200000000000000391f6900000d42030001feff0002043458000000003c745e00000d402b001afeff00ff0002000000000000003d3b7200000d4207003efeff00020c34580000000039076e00000d4023005cfeff00ff00020000000000000038411900000d420f0022feff000204345800000000477e7700000d402b0042feff00ff00020000000000000048456a00000d420b0021feff00020434245400000049006d00000d400b0020feff00ff000200000000000000493c3100000d4203000afeff0002043458000000004d444b00000d402f004afe
|
||||||
|
//ff00ffffffff000200000000000000112d6f00000c42070018fe
|
||||||
|
QList<QByteArray> validFrames;
|
||||||
|
int start = 0;
|
||||||
|
|
||||||
|
while (start < buffer.size())
|
||||||
|
{
|
||||||
|
// <20><><EFBFBD><EFBFBD>֡ͷ 'ff'
|
||||||
|
int frameStart = buffer.indexOf(MOTOR_SYNC, start);
|
||||||
|
if (buffer.at(frameStart + 1) == MOTOR_SYNC)
|
||||||
|
continue;
|
||||||
|
if (frameStart == -1)
|
||||||
|
{
|
||||||
|
break; // û<><C3BB><EFBFBD>ҵ<EFBFBD>֡ͷ<D6A1><CDB7><EFBFBD>˳<EFBFBD>ѭ<EFBFBD><D1AD>
|
||||||
|
}
|
||||||
|
|
||||||
|
// <20><><EFBFBD><EFBFBD>֡β 'fe'
|
||||||
|
int frameEnd = buffer.indexOf(MOTOR_ETX, frameStart + 1);
|
||||||
|
if (frameEnd == -1)
|
||||||
|
{
|
||||||
|
break; // û<><C3BB><EFBFBD>ҵ<EFBFBD>֡β<D6A1><CEB2><EFBFBD>˳<EFBFBD>ѭ<EFBFBD><D1AD>
|
||||||
|
}
|
||||||
|
|
||||||
|
// <20><>ȡ<EFBFBD><C8A1>Ч֡<D0A7><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֡ͷ<D6A1><CDB7>֡β<D6A1><CEB2>
|
||||||
|
QByteArray frame = buffer.mid(frameStart, frameEnd - frameStart + 1);
|
||||||
|
|
||||||
|
// <20><><EFBFBD><EFBFBD>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>21
|
||||||
|
if (frame.size() == 21)
|
||||||
|
{
|
||||||
|
validFrames.append(frame);
|
||||||
|
}
|
||||||
|
|
||||||
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>֡
|
||||||
|
start = frameEnd + 1;
|
||||||
|
}
|
||||||
|
if (validFrames.size() == 0)
|
||||||
|
{
|
||||||
|
std::cout << "error--------------: " << buffer.toHex().toStdString() << std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
errorCode = MOTOR_INVALID_FRAME;
|
||||||
|
return errorCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
buffer = validFrames.at(validFrames.size() - 1);
|
||||||
|
|
||||||
|
//<2F><>ʽ3<CABD><33><EFBFBD>ӽ<EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>Ч֡<D0A7><D6A1>1<EFBFBD><31><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD>fe<66><65>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>fe<66><65>ʼ<EFBFBD><CABC>ǰ<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>Ч֡<D0A7><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ21<32><31>
|
||||||
|
|
||||||
|
//std::cout << "after" << buffer.size() << std::endl;
|
||||||
|
//std::cout << buffer.toHex().toStdString() << std::endl;
|
||||||
|
|
||||||
|
return errorCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
IrisMotorErrorCode VinceMotorControllerUsb::parseOneValidFrame(QByteArray& buf, State& backdatt)
|
||||||
|
{
|
||||||
|
IrisMotorErrorCode errorCode = MOTOR_NO_ERROR;
|
||||||
|
|
||||||
|
backdatt.Speed = MOTOR_disconnection;
|
||||||
|
|
||||||
|
buf.remove(0, 3);
|
||||||
|
buf.remove(buf.length() - 3, 3);
|
||||||
|
int location;
|
||||||
|
unsigned char a[15];
|
||||||
|
memcpy(a, buf.data(), 15);
|
||||||
|
location = ((a[0] & 0x0f) << 28) | ((a[1] & 0x7f) << 21) | ((a[2] & 0x7f) << 14) | ((a[3] & 0x7f) << 7) | ((a[4] & 0x7f));
|
||||||
|
float speed;
|
||||||
|
|
||||||
|
memcpy(&speed, &location, 4);
|
||||||
|
backdatt.Speed = speed;
|
||||||
|
int loc;
|
||||||
|
loc = ((a[5] & 0x0f) << 28) | ((a[6] & 0x7f) << 21) | ((a[7] & 0x7f) << 14) | ((a[8] & 0x7f) << 7) | ((a[9] & 0x7f));
|
||||||
|
backdatt.Location = loc;
|
||||||
|
backdatt.Speed = speed;
|
||||||
|
location = ((a[10] & 0x0f) << 28) | ((a[11] & 0x7f) << 21) | ((a[12] & 0x7f) << 14) | ((a[13] & 0x7f) << 7) | ((a[14] & 0x7f));
|
||||||
|
backdatt.Stata = unsigned int(location);
|
||||||
|
|
||||||
|
/*QString str1 = "Speed " + QString::number(backdatt.Speed, 'f', 2) + " location " + QString::number(backdatt.Location);
|
||||||
|
qDebug() << str1;*/
|
||||||
|
|
||||||
|
return errorCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
IrisMotorErrorCode VinceMotorControllerUsb::parseBytedata(QByteArray buf, State& backdatt)
|
||||||
|
{
|
||||||
|
IrisMotorErrorCode errorCode = extractOneValidFrame(buf);
|
||||||
|
|
||||||
|
backdatt.Speed = MOTOR_disconnection;
|
||||||
|
backdatt.Location = 0;
|
||||||
|
if (errorCode == MOTOR_NO_ERROR)
|
||||||
|
{
|
||||||
|
parseOneValidFrame(buf, backdatt);
|
||||||
|
return errorCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
errorCode = MOTOR_INVALID_FRAME;
|
||||||
|
return errorCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::init(QString jsonPath)
|
||||||
|
{
|
||||||
|
enable();
|
||||||
|
|
||||||
|
JsonOperate xxx(jsonPath);
|
||||||
|
//xxx.createJsonFile();
|
||||||
|
QStringList re = xxx.parseJsonFile();
|
||||||
|
|
||||||
|
for (const QString& pair : re)
|
||||||
|
{
|
||||||
|
//qDebug().noquote() << pair;
|
||||||
|
sendCommand2Motor(pair);
|
||||||
|
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
//QString cmd;
|
||||||
|
|
||||||
|
////cmd = "cfg s1f=2\n";//<2F>ò<EFBFBD><C3B2><EFBFBD>
|
||||||
|
////sendCommand2Motor(cmd);
|
||||||
|
|
||||||
|
////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
//cmd = "cfg zmd=2\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg snr=0\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg osv=0\n";//<2F><><EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg zsd=3000\n";//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg zsp=2400\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
|
||||||
|
////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
//cmd = "cfg msr=1\n";//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg msv=0\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg psr=2\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg psv=0\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
|
||||||
|
////<2F>Ӽ<EFBFBD><D3BC>ٶȺ͵<C8BA><CDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
//cmd = "cfg mcs=7\n";//<2F><><EFBFBD><EFBFBD>ϸ<EFBFBD><CFB8>
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg acc=20000\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg dec=20000\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg crn=4\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg cra=4\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
//cmd = "cfg crh=1\n";
|
||||||
|
//sendCommand2Motor(cmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VinceMotorControllerUsb::isConnected()
|
||||||
|
{
|
||||||
|
State re = getState();
|
||||||
|
if (re.Speed == MOTOR_disconnection)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::enable()
|
||||||
|
{
|
||||||
|
QString cmd = "ena\n";
|
||||||
|
sendCommand2Motor(cmd);
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::move2Pos(double pos)
|
||||||
|
{
|
||||||
|
QString cmd = "pos " + QString::number(pos, 'f', 10) + "\n";
|
||||||
|
sendCommand2Motor(cmd);
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::move(bool towardOrigin)
|
||||||
|
{
|
||||||
|
if (towardOrigin)
|
||||||
|
{
|
||||||
|
setSpeed(abs(m_iSpeed) * -1);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
setSpeed(abs(m_iSpeed));
|
||||||
|
}
|
||||||
|
|
||||||
|
sendCommand2Motor("mov\n");
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::stopMove()
|
||||||
|
{
|
||||||
|
QString cmd = "stp\n";
|
||||||
|
sendCommand2Motor(cmd);
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::setSpeed(double speed)
|
||||||
|
{
|
||||||
|
m_iSpeed = speed;
|
||||||
|
QString cmd = "cfg spd=" + QString::number(speed) + "\n";
|
||||||
|
sendCommand2Motor(cmd);
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
double VinceMotorControllerUsb::getSpeed()
|
||||||
|
{
|
||||||
|
State re = getState();
|
||||||
|
return re.Speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
double VinceMotorControllerUsb::getCurrentLoc()
|
||||||
|
{
|
||||||
|
State re = getState();
|
||||||
|
return re.Location;
|
||||||
|
}
|
||||||
|
|
||||||
|
void VinceMotorControllerUsb::zeroStart()
|
||||||
|
{
|
||||||
|
QString cmd = "zero start\n";
|
||||||
|
sendCommand2Motor(cmd);
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
}
|
||||||
|
|
||||||
|
int VinceMotorControllerUsb::getSpeedLocation(double& speed, double& locatioin)
|
||||||
|
{
|
||||||
|
State re = getState();
|
||||||
|
speed = re.Speed;
|
||||||
|
locatioin = re.Location;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
State VinceMotorControllerUsb::getState()
|
||||||
|
{
|
||||||
|
QString cmd = "sts\n";
|
||||||
|
sendCommand2Motor(cmd);
|
||||||
|
|
||||||
|
QByteArray buf;
|
||||||
|
recvData(buf);
|
||||||
|
//std::cout << buf.size() << std::endl;
|
||||||
|
//std::cout << buf.toHex().toStdString() << std::endl;
|
||||||
|
|
||||||
|
State back;
|
||||||
|
IrisMotorErrorCode errorCode = parseBytedata(buf, back);
|
||||||
|
|
||||||
|
return back;
|
||||||
|
}
|
||||||
50
VinceMotorControllerUsb.h
Normal file
50
VinceMotorControllerUsb.h
Normal file
@ -0,0 +1,50 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <QObject>
|
||||||
|
#include <QString>
|
||||||
|
#include <QtSerialPort/QSerialPort>
|
||||||
|
#include <QDebug>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
#include "JsonOperate.h"
|
||||||
|
#include "MotorControllerBase.h"
|
||||||
|
|
||||||
|
#define MOTOR_SYNC (0xFF)//֡ͷ
|
||||||
|
#define MOTOR_ETX (0xFE)//֡β
|
||||||
|
|
||||||
|
#define MOTOR_disconnection -1000000//<2F>ٶȵ<D9B6><C8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CDB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨѶ<CDA8>쳣
|
||||||
|
|
||||||
|
class VinceMotorControllerUsb :
|
||||||
|
public MotorControllerBase, QObject
|
||||||
|
{
|
||||||
|
Q_OBJECT
|
||||||
|
|
||||||
|
public:
|
||||||
|
VinceMotorControllerUsb(QString serialPort, QString paramJson, QObject* parent = nullptr);
|
||||||
|
~VinceMotorControllerUsb();
|
||||||
|
|
||||||
|
void init(QString jsonPath);
|
||||||
|
int sendCommand2Motor(QString cmd);
|
||||||
|
bool isConnected();
|
||||||
|
void enable();
|
||||||
|
|
||||||
|
void move2Pos(double pos);
|
||||||
|
void move(bool towardOrigin);//true<75><65>Զ<EFBFBD><D4B6>ԭ<EFBFBD>㣬false<73><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD>
|
||||||
|
void stopMove();
|
||||||
|
void setSpeed(double speed);
|
||||||
|
double getSpeed();
|
||||||
|
double getCurrentLoc();
|
||||||
|
void zeroStart();
|
||||||
|
|
||||||
|
int getSpeedLocation(double& speed, double& locatioin);
|
||||||
|
|
||||||
|
State getState();
|
||||||
|
int recvData(QByteArray& dataRecv);
|
||||||
|
IrisMotorErrorCode parseBytedata(QByteArray buf, State& backdatt);
|
||||||
|
IrisMotorErrorCode extractOneValidFrame(QByteArray& buffer);
|
||||||
|
IrisMotorErrorCode parseOneValidFrame(QByteArray& buffer, State& result);
|
||||||
|
|
||||||
|
private:
|
||||||
|
QSerialPort* m_pSerialPort;
|
||||||
|
|
||||||
|
double m_iSpeed;
|
||||||
|
};
|
||||||
65
device_config.json
Normal file
65
device_config.json
Normal file
@ -0,0 +1,65 @@
|
|||||||
|
{
|
||||||
|
"cmd": "cfg",
|
||||||
|
"params": {
|
||||||
|
"cfg zmd": 1,
|
||||||
|
"cfg snr": 0,
|
||||||
|
"cfg osv": 0,
|
||||||
|
"cfg zsd": 1200,
|
||||||
|
"cfg zsp": 2400,
|
||||||
|
"cfg dmd": 2,
|
||||||
|
"cfg dar": 5,
|
||||||
|
"cfg msr": 1,
|
||||||
|
"cfg msv": 1,
|
||||||
|
"cfg psr": 1,
|
||||||
|
"cfg psv": 1,
|
||||||
|
"cfg bdr": 115200,
|
||||||
|
"cfg cid": 1,
|
||||||
|
"cfg mcs": 5,
|
||||||
|
"cfg spd": 1200,
|
||||||
|
"cfg acc": 12000,
|
||||||
|
"cfg dec": 12000,
|
||||||
|
"cfg cra": 0.8,
|
||||||
|
"cfg crn": 0.4,
|
||||||
|
"cfg crh": 0.0
|
||||||
|
},
|
||||||
|
"actions": [
|
||||||
|
{
|
||||||
|
"cmd": "ena"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "off"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "mov"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "pos",
|
||||||
|
"value": 10000
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "rmv",
|
||||||
|
"value": -6400
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "pps"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "pps",
|
||||||
|
"value": 12800
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "org"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "stp"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "zero",
|
||||||
|
"value": "start"
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cmd": "zero",
|
||||||
|
"value": "stop"
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
37
include/README
Normal file
37
include/README
Normal file
@ -0,0 +1,37 @@
|
|||||||
|
|
||||||
|
This directory is intended for project header files.
|
||||||
|
|
||||||
|
A header file is a file containing C declarations and macro definitions
|
||||||
|
to be shared between several project source files. You request the use of a
|
||||||
|
header file in your project source file (C, C++, etc) located in `src` folder
|
||||||
|
by including it, with the C preprocessing directive `#include'.
|
||||||
|
|
||||||
|
```src/main.c
|
||||||
|
|
||||||
|
#include "header.h"
|
||||||
|
|
||||||
|
int main (void)
|
||||||
|
{
|
||||||
|
...
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
Including a header file produces the same results as copying the header file
|
||||||
|
into each source file that needs it. Such copying would be time-consuming
|
||||||
|
and error-prone. With a header file, the related declarations appear
|
||||||
|
in only one place. If they need to be changed, they can be changed in one
|
||||||
|
place, and programs that include the header file will automatically use the
|
||||||
|
new version when next recompiled. The header file eliminates the labor of
|
||||||
|
finding and changing all the copies as well as the risk that a failure to
|
||||||
|
find one copy will result in inconsistencies within a program.
|
||||||
|
|
||||||
|
In C, the convention is to give header files names that end with `.h'.
|
||||||
|
|
||||||
|
Read more about using header files in official GCC documentation:
|
||||||
|
|
||||||
|
* Include Syntax
|
||||||
|
* Include Operation
|
||||||
|
* Once-Only Headers
|
||||||
|
* Computed Includes
|
||||||
|
|
||||||
|
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
|
||||||
46
lib/README
Normal file
46
lib/README
Normal file
@ -0,0 +1,46 @@
|
|||||||
|
|
||||||
|
This directory is intended for project specific (private) libraries.
|
||||||
|
PlatformIO will compile them to static libraries and link into the executable file.
|
||||||
|
|
||||||
|
The source code of each library should be placed in a separate directory
|
||||||
|
("lib/your_library_name/[Code]").
|
||||||
|
|
||||||
|
For example, see the structure of the following example libraries `Foo` and `Bar`:
|
||||||
|
|
||||||
|
|--lib
|
||||||
|
| |
|
||||||
|
| |--Bar
|
||||||
|
| | |--docs
|
||||||
|
| | |--examples
|
||||||
|
| | |--src
|
||||||
|
| | |- Bar.c
|
||||||
|
| | |- Bar.h
|
||||||
|
| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
|
||||||
|
| |
|
||||||
|
| |--Foo
|
||||||
|
| | |- Foo.c
|
||||||
|
| | |- Foo.h
|
||||||
|
| |
|
||||||
|
| |- README --> THIS FILE
|
||||||
|
|
|
||||||
|
|- platformio.ini
|
||||||
|
|--src
|
||||||
|
|- main.c
|
||||||
|
|
||||||
|
Example contents of `src/main.c` using Foo and Bar:
|
||||||
|
```
|
||||||
|
#include <Foo.h>
|
||||||
|
#include <Bar.h>
|
||||||
|
|
||||||
|
int main (void)
|
||||||
|
{
|
||||||
|
...
|
||||||
|
}
|
||||||
|
|
||||||
|
```
|
||||||
|
|
||||||
|
The PlatformIO Library Dependency Finder will find automatically dependent
|
||||||
|
libraries by scanning project source files.
|
||||||
|
|
||||||
|
More information about PlatformIO Library Dependency Finder
|
||||||
|
- https://docs.platformio.org/page/librarymanager/ldf.html
|
||||||
2650
main_test.cpp
Normal file
2650
main_test.cpp
Normal file
File diff suppressed because it is too large
Load Diff
19
platformio - 副本.ini
Normal file
19
platformio - 副本.ini
Normal file
@ -0,0 +1,19 @@
|
|||||||
|
; PlatformIO Project Configuration File
|
||||||
|
;
|
||||||
|
; Build options: build flags, source filter
|
||||||
|
; Upload options: custom upload port, speed and extra flags
|
||||||
|
; Library options: dependencies, extra library storages
|
||||||
|
; Advanced options: extra scripting
|
||||||
|
;
|
||||||
|
; Please visit documentation for the other options and examples
|
||||||
|
; https://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
|
[env:esp32-s3-devkitc-1]
|
||||||
|
platform = espressif32 @ ^4.4.0
|
||||||
|
board = esp32-s3-devkitc-1
|
||||||
|
framework = arduino
|
||||||
|
platform_packages =
|
||||||
|
toolchain-riscv32-esp @ 8.4.0
|
||||||
|
board_upload.flash_size = 4MB
|
||||||
|
board_upload.psram_size = 2MB
|
||||||
|
|
||||||
15
platformio.ini
Normal file
15
platformio.ini
Normal file
@ -0,0 +1,15 @@
|
|||||||
|
; PlatformIO Project Configuration File
|
||||||
|
;
|
||||||
|
; Build options: build flags, source filter
|
||||||
|
; Upload options: custom upload port, speed and extra flags
|
||||||
|
; Library options: dependencies, extra library storages
|
||||||
|
; Advanced options: extra scripting
|
||||||
|
;
|
||||||
|
; Please visit documentation for the other options and examples
|
||||||
|
; https://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
|
[env:esp32s3box]
|
||||||
|
platform = http://111.198.24.44:11080/v6.5.0.zip
|
||||||
|
board = esp32-s3-devkitc-1
|
||||||
|
framework = arduino
|
||||||
|
lib_deps = bblanchon/ArduinoJson@^7.4.2
|
||||||
208
src/IRIS_Method.c
Normal file
208
src/IRIS_Method.c
Normal file
@ -0,0 +1,208 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @file : IRIS_Method.c
|
||||||
|
* @author : xin
|
||||||
|
* @brief : None
|
||||||
|
* @attention : None
|
||||||
|
* @date : 2024/2/1
|
||||||
|
******************************************************************************
|
||||||
|
*/
|
||||||
|
|
||||||
|
//
|
||||||
|
// Created by xin on 2024/2/1.
|
||||||
|
//
|
||||||
|
#include "IRIS_Method.h"
|
||||||
|
|
||||||
|
// 成功返回打包后的数据长度
|
||||||
|
// -1: Error
|
||||||
|
int32_t IRIS_Protocol_Pack(uint8_t Command, uint16_t LenthofIn, uint8_t *BufferIn, uint8_t *PackData) {
|
||||||
|
if (PackData == NULL || (LenthofIn != 0 && BufferIn == NULL)) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
PackData[0] = 0x55;
|
||||||
|
PackData[1] = 0xAA;
|
||||||
|
PackData[2] = Command;
|
||||||
|
uint16_t datalenth = LenthofIn;
|
||||||
|
PackData[3] = (datalenth >> 8) & 0xFF;
|
||||||
|
PackData[4] = datalenth & 0xFF;
|
||||||
|
|
||||||
|
if (LenthofIn != 0) {
|
||||||
|
memcpy(&PackData[5], BufferIn, LenthofIn);
|
||||||
|
}
|
||||||
|
uint16_t crcbytelenth = LenthofIn;
|
||||||
|
uint16_t CRC = IRIS_calcCRC(PackData + 5, crcbytelenth);
|
||||||
|
PackData[LenthofIn + 5] = (CRC >> 8) & 0xFF;
|
||||||
|
PackData[LenthofIn + 6] = CRC & 0xFF;
|
||||||
|
return LenthofIn + 7;
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
// int32_t IRIS_STM32_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t *Command, uint8_t *BufferOut) {
|
||||||
|
// if (PackData == NULL || BufferOut == NULL) {
|
||||||
|
// return ERROR_INPUT;
|
||||||
|
// }
|
||||||
|
// if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
|
||||||
|
// return ERROR_HEADER;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// uint16_t LenthofOut = PackData[4] + (PackData[3] << 8);
|
||||||
|
|
||||||
|
// // 修改长度检查：确保有足够的数据包含头部、数据和校验
|
||||||
|
// if (LenthofIn < 5 + LenthofOut + 2) {
|
||||||
|
// return ERROR_NOT_ENOUGH_DATA;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // 修正校验标识位置：应该是 PackData[5 + LenthofOut] 和 PackData[5 + LenthofOut + 1]
|
||||||
|
// if (PackData[5 + LenthofOut] == 0xEE && PackData[5 + LenthofOut + 1] == 0xEE) {
|
||||||
|
// // 特殊校验标识，跳过CRC检查
|
||||||
|
// } else {
|
||||||
|
// uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
|
||||||
|
// if (CRC != (PackData[5 + LenthofOut + 1] + (PackData[5 + LenthofOut] << 8))) {
|
||||||
|
// return ERROR_CRC;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// if (LenthofOut == 0) {
|
||||||
|
// return 0;
|
||||||
|
// }
|
||||||
|
// *Command = PackData[2];
|
||||||
|
|
||||||
|
// memcpy(BufferOut, &PackData[5], LenthofOut);
|
||||||
|
// return LenthofOut;
|
||||||
|
// }
|
||||||
|
int32_t IRIS_STM32_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t *Command, uint8_t *BufferOut) {
|
||||||
|
|
||||||
|
if (PackData == NULL || BufferOut == NULL) {
|
||||||
|
return ERROR_INPUT;
|
||||||
|
}
|
||||||
|
if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
|
||||||
|
return ERROR_HEADER;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t LenthofOut = PackData[4] + (PackData[3] << 8); //减去CRC的两个字节
|
||||||
|
if (LenthofOut > LenthofIn - 7) {
|
||||||
|
return ERROR_NOT_ENOUGH_DATA;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (PackData[LenthofOut + 6] == 0xEE && PackData[LenthofOut + 5] == 0xEE) {
|
||||||
|
|
||||||
|
} else {
|
||||||
|
uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
|
||||||
|
if (CRC != (PackData[LenthofOut + 6] + (PackData[LenthofOut + 5] << 8))) {
|
||||||
|
return ERROR_CRC;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (LenthofOut == 0) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
*Command = PackData[2];
|
||||||
|
|
||||||
|
memcpy(BufferOut, &PackData[5], LenthofOut);
|
||||||
|
return LenthofOut;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int32_t IRIS_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t Command, uint8_t *BufferOut) {
|
||||||
|
if (PackData == NULL || BufferOut == NULL) {
|
||||||
|
return ERROR_INPUT;
|
||||||
|
}
|
||||||
|
if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
|
||||||
|
return ERROR_HEADER;
|
||||||
|
}
|
||||||
|
if (PackData[2] != Command) {
|
||||||
|
return ERROR_COMMAND;
|
||||||
|
}
|
||||||
|
uint16_t LenthofOut = PackData[4] + (PackData[3] << 8);
|
||||||
|
if (LenthofOut > LenthofIn - 7) {
|
||||||
|
return ERROR_NOT_ENOUGH_DATA;
|
||||||
|
}
|
||||||
|
if (PackData[LenthofOut + 6] == 0xEE && PackData[LenthofOut + 5] == 0xEE) {
|
||||||
|
|
||||||
|
} else {
|
||||||
|
uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
|
||||||
|
if (CRC != (PackData[LenthofOut + 6] + (PackData[LenthofOut + 5] << 8))) {
|
||||||
|
return ERROR_CRC;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (LenthofOut == 0) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
memcpy(BufferOut, &PackData[5], LenthofOut);
|
||||||
|
return LenthofOut;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int32_t IRIS_Cut_Befor_Header(uint8_t *PackData, uint16_t LenthofIn) {
|
||||||
|
if (PackData == NULL) {
|
||||||
|
return ERROR_INPUT;
|
||||||
|
}
|
||||||
|
uint16_t i = 0;
|
||||||
|
for (i = 0; i < LenthofIn; i++) {
|
||||||
|
if (PackData[i] == 0x55 && PackData[i + 1] == 0xAA) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (i == LenthofIn) {
|
||||||
|
//清空数据
|
||||||
|
memset(PackData, 0, LenthofIn);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t LenthofOut = LenthofIn - i;
|
||||||
|
memcpy(PackData, &PackData[i], LenthofOut);
|
||||||
|
return LenthofOut;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32_t IRIS_Check_Data_Valid(uint8_t *PackData, uint16_t LenthofIn) {
|
||||||
|
if (PackData == NULL) {
|
||||||
|
return ERROR_INPUT;
|
||||||
|
}
|
||||||
|
if (LenthofIn < 7) {
|
||||||
|
return ERROR_NOT_ENOUGH_DATA;
|
||||||
|
/* code */
|
||||||
|
}
|
||||||
|
|
||||||
|
if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
|
||||||
|
return ERROR_HEADER;
|
||||||
|
}
|
||||||
|
uint16_t LenthofOut = PackData[4] + (PackData[3] << 8);
|
||||||
|
if (LenthofOut > LenthofIn - 7) {
|
||||||
|
return ERROR_NOT_ENOUGH_DATA;
|
||||||
|
}
|
||||||
|
if (PackData[LenthofOut + 6] == 0xEE && PackData[LenthofOut + 5] == 0xEE) {
|
||||||
|
|
||||||
|
} else {
|
||||||
|
uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
|
||||||
|
if (CRC != (PackData[LenthofOut + 6] + (PackData[LenthofOut + 5] << 8))) {
|
||||||
|
return ERROR_CRC;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 1;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
uint16_t IRIS_calcCRC(const void *pBuffer, uint16_t bufferSize) {
|
||||||
|
const uint8_t *pBytesArray = (const uint8_t *) pBuffer;
|
||||||
|
uint16_t poly = 0x8408;
|
||||||
|
uint16_t crc = 0;
|
||||||
|
uint8_t carry;
|
||||||
|
uint8_t i_bits;
|
||||||
|
uint16_t j;
|
||||||
|
for (j = 0; j < bufferSize; j++) {
|
||||||
|
crc = crc ^ pBytesArray[j];
|
||||||
|
for (i_bits = 0; i_bits < 8; i_bits++) {
|
||||||
|
carry = crc & 1;
|
||||||
|
crc = crc / 2;
|
||||||
|
if (carry) {
|
||||||
|
crc = crc ^ poly;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return crc;
|
||||||
|
}
|
||||||
|
|
||||||
66
src/IRIS_Method.h
Normal file
66
src/IRIS_Method.h
Normal file
@ -0,0 +1,66 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @file : IRIS_Method.h
|
||||||
|
* @author : xin
|
||||||
|
* @brief : None
|
||||||
|
* @attention : None
|
||||||
|
* @date : 2024/2/1
|
||||||
|
******************************************************************************
|
||||||
|
*/
|
||||||
|
|
||||||
|
//
|
||||||
|
// Created by xin on 2024/2/1.
|
||||||
|
//
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// #ifndef IRIS_COMMUNICATION_PROTOCOL_IRIS_METHOD_H
|
||||||
|
// #define IRIS_COMMUNICATION_PROTOCOL_IRIS_METHOD_H
|
||||||
|
|
||||||
|
|
||||||
|
#define ERROR_NOT_ENOUGH_DATA -200
|
||||||
|
#define ERROR_HEADER -300
|
||||||
|
#define ERROR_COMMAND -400
|
||||||
|
#define ERROR_INPUT -500
|
||||||
|
#define ERROR_CRC -600
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
// 成功返回打包后的数据长度
|
||||||
|
// -1: Error
|
||||||
|
int32_t IRIS_Protocol_Pack(uint8_t Command, uint16_t LenthofIn, uint8_t *BufferIn, uint8_t *PackData);
|
||||||
|
|
||||||
|
// 解包函数 PackData 是接收到的数据 LenthofIn 是数据长度 Command 是命令 BufferOut 是输出
|
||||||
|
// 下位机使用的打包函数 Command 是输出
|
||||||
|
// 成功返回解包后的数据长度
|
||||||
|
// 0: 该命令返回无参数
|
||||||
|
// 错误返回ERRor
|
||||||
|
// 成功返回解包后的数据长度
|
||||||
|
int32_t IRIS_STM32_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t *Command, uint8_t *BufferOut);
|
||||||
|
|
||||||
|
// 解包函数 PackData 是接收到的数据 LenthofIn 是数据长度 Command 是命令输入 BufferOut 是输出 上位机使用
|
||||||
|
// 成功返回解包后的数据长度
|
||||||
|
// 0: 该命令返回无参数
|
||||||
|
// 错误返回ERRor
|
||||||
|
// 成功返回解包后的数据长度
|
||||||
|
int32_t IRIS_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t Command, uint8_t *BufferOut);
|
||||||
|
|
||||||
|
// 定义裁切命令
|
||||||
|
// 成功返回裁切后的数据长度
|
||||||
|
// -1: Error
|
||||||
|
int32_t IRIS_Cut_Befor_Header(uint8_t *PackData, uint16_t LenthofIn);
|
||||||
|
|
||||||
|
// 检查数据是否有效
|
||||||
|
// 有效返回值1
|
||||||
|
// 错误返回ERRor
|
||||||
|
int32_t IRIS_Check_Data_Valid(uint8_t *PackData, uint16_t LenthofIn);
|
||||||
|
|
||||||
|
// 返回CRC校验值
|
||||||
|
uint16_t IRIS_calcCRC(const void *pBuffer, uint16_t bufferSize);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
85
src/ble.cpp
Normal file
85
src/ble.cpp
Normal file
@ -0,0 +1,85 @@
|
|||||||
|
#include "ble.h"
|
||||||
|
|
||||||
|
// 定义 BLE 服务和特征值的 UUID
|
||||||
|
#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
|
||||||
|
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
|
||||||
|
|
||||||
|
BLEServer* pServer = nullptr;
|
||||||
|
BLECharacteristic* pCharacteristic = nullptr;
|
||||||
|
BLEAdvertising* pAdvertising;
|
||||||
|
|
||||||
|
bool isClientConnected = false;
|
||||||
|
|
||||||
|
class MyServerCallbacks : public BLEServerCallbacks {
|
||||||
|
void onConnect(BLEServer* pServer) {
|
||||||
|
Serial.println("CONNECT");
|
||||||
|
isClientConnected = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void onDisconnect(BLEServer* pServer) {
|
||||||
|
Serial.println("DISCONNECT");
|
||||||
|
isClientConnected = false;
|
||||||
|
// 重新启动广播，使设备再次可被发现
|
||||||
|
BLEDevice::startAdvertising();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
bool bleSentData = false; // 记录 BLE 是否已发送数据
|
||||||
|
|
||||||
|
class MyCharacteristicCallbacks : public BLECharacteristicCallbacks {
|
||||||
|
void onWrite(BLECharacteristic* pCharacteristic) {
|
||||||
|
std::string value = pCharacteristic->getValue();
|
||||||
|
|
||||||
|
if (value.length() > 0) {
|
||||||
|
bleSentData = true;
|
||||||
|
//Serial.print("Receive: ");
|
||||||
|
for (int i = 0; i < value.length(); i++) {
|
||||||
|
Serial.print(value[i]);
|
||||||
|
}
|
||||||
|
Serial.println();
|
||||||
|
}
|
||||||
|
//bleSentData = false;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void bleInit() {
|
||||||
|
// 初始化 BLE 设备
|
||||||
|
BLEDevice::init("VSMD_BLE"); // 设置设备名称
|
||||||
|
|
||||||
|
// 创建 BLE 服务器
|
||||||
|
pServer = BLEDevice::createServer();
|
||||||
|
pServer->setCallbacks(new MyServerCallbacks());
|
||||||
|
|
||||||
|
// 创建 BLE 服务
|
||||||
|
BLEService* pService = pServer->createService(SERVICE_UUID);
|
||||||
|
|
||||||
|
// 创建 BLE 特征值
|
||||||
|
pCharacteristic = pService->createCharacteristic(
|
||||||
|
CHARACTERISTIC_UUID,
|
||||||
|
BLECharacteristic::PROPERTY_READ |
|
||||||
|
BLECharacteristic::PROPERTY_WRITE
|
||||||
|
);
|
||||||
|
|
||||||
|
// 添加特征值描述
|
||||||
|
pCharacteristic->setCallbacks(new MyCharacteristicCallbacks());
|
||||||
|
//pCharacteristic->setValue("Hello Client");
|
||||||
|
|
||||||
|
// 启动服务
|
||||||
|
pService->start();
|
||||||
|
|
||||||
|
// 创建广告数据
|
||||||
|
BLEAdvertising* pAdvertising = BLEDevice::getAdvertising();
|
||||||
|
pAdvertising->addServiceUUID(BLEUUID(SERVICE_UUID));
|
||||||
|
pAdvertising->setScanResponse(true);
|
||||||
|
pAdvertising->setMinPreferred(0x06);
|
||||||
|
BLEDevice::startAdvertising();
|
||||||
|
|
||||||
|
Serial.println("Waiting for client connection...");
|
||||||
|
|
||||||
|
// 在初始化时设置回调
|
||||||
|
pServer->setCallbacks(new MyServerCallbacks());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
14
src/ble.h
Normal file
14
src/ble.h
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
#ifndef BLE_H
|
||||||
|
#define BLE_H
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <BLEDevice.h>
|
||||||
|
#include <BLEUtils.h>
|
||||||
|
#include <BLEServer.h>
|
||||||
|
#include <BLECharacteristic.h>
|
||||||
|
|
||||||
|
|
||||||
|
// 蓝牙初始化函数
|
||||||
|
void bleInit();
|
||||||
|
|
||||||
|
#endif
|
||||||
493
src/main.cpp
Normal file
493
src/main.cpp
Normal file
@ -0,0 +1,493 @@
|
|||||||
|
#include <ArduinoJson.h>
|
||||||
|
#include "IRIS_Method.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define DEBUG_TX_PIN 17 // TX
|
||||||
|
#define DEBUG_RX_PIN 16 // RX
|
||||||
|
|
||||||
|
|
||||||
|
HardwareSerial DebugSerial(1); // 使用UART1
|
||||||
|
|
||||||
|
#define BUFFER_SIZE 1024
|
||||||
|
#define JSON_BUFFER_SIZE 512
|
||||||
|
|
||||||
|
// 全局变量声明
|
||||||
|
uint8_t receiveBuffer[BUFFER_SIZE]; // 接收缓冲区
|
||||||
|
uint8_t sendBuffer[BUFFER_SIZE]; // 发送缓冲区
|
||||||
|
uint8_t responseBuffer[BUFFER_SIZE]; // 响应缓冲区
|
||||||
|
uint8_t command; // 存储命令
|
||||||
|
int32_t bytesProcessed; // 处理的数据字节数
|
||||||
|
int32_t unpackResult; // 解包结果
|
||||||
|
int32_t packedLength; // 打包后的数据长度
|
||||||
|
|
||||||
|
// 设备状态变量
|
||||||
|
bool deviceEnabled = false;
|
||||||
|
int32_t currentPosition = 0;
|
||||||
|
int32_t targetPosition = 0;
|
||||||
|
int32_t pulsePerSecond = 1200;
|
||||||
|
bool isMoving = false;
|
||||||
|
bool isBackZero = false;
|
||||||
|
|
||||||
|
// 设备配置参数
|
||||||
|
struct DeviceConfig {
|
||||||
|
int zmd = 1; // cfg zmd
|
||||||
|
int snr = 0; // cfg snr
|
||||||
|
int osv = 0; // cfg osv
|
||||||
|
int zsd = 1200; // cfg zsd
|
||||||
|
int zsp = 2400; // cfg zsp
|
||||||
|
int dmd = 2; // cfg dmd
|
||||||
|
int dar = 5; // cfg dar
|
||||||
|
int msr = 1; // cfg msr
|
||||||
|
int msv = 1; // cfg msv
|
||||||
|
int psr = 1; // cfg psr
|
||||||
|
int psv = 1; // cfg psv
|
||||||
|
int bdr = 115200; // cfg bdr
|
||||||
|
int cid = 1; // cfg cid
|
||||||
|
int mcs = 5; // cfg mcs
|
||||||
|
int spd = 1200; // cfg spd
|
||||||
|
int acc = 12000; // cfg acc
|
||||||
|
int dec = 12000; // cfg dec
|
||||||
|
float cra = 0.8; // cfg cra
|
||||||
|
float crn = 0.4; // cfg crn
|
||||||
|
float crh = 0.0; // cfg crh
|
||||||
|
} deviceConfig;
|
||||||
|
|
||||||
|
|
||||||
|
void processJsonCommand(const char* jsonString);
|
||||||
|
void processStringCommand(const char* stringData);
|
||||||
|
void sendJsonResponse(const char* cmd, const char* status, int value = -1);
|
||||||
|
void sendStringResponse(const char* response);
|
||||||
|
void handleConfigCommand(JsonObject& doc);
|
||||||
|
void handleEnableCommand();
|
||||||
|
void handleOffCommand();
|
||||||
|
void handleMoveCommand();
|
||||||
|
void handlePositionCommand(int value);
|
||||||
|
void handleRelativeMoveCommand(int value);
|
||||||
|
void handlePulsePerSecondCommand(int value = -1);
|
||||||
|
void handleOriginCommand();
|
||||||
|
void handleStopCommand();
|
||||||
|
void handleZeroCommand(const char* value);
|
||||||
|
void printDeviceStatus();
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
// 调试串口
|
||||||
|
DebugSerial.begin(115200, SERIAL_8N1, DEBUG_RX_PIN, DEBUG_TX_PIN);
|
||||||
|
|
||||||
|
// 主串口（用于数据通信）
|
||||||
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
DebugSerial.println("VSMD INIT");
|
||||||
|
DebugSerial.println("Commands: cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
|
||||||
|
printDeviceStatus();
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
if (DebugSerial.available() > 0) {
|
||||||
|
int bytesRead = DebugSerial.readBytes(receiveBuffer, sizeof(receiveBuffer));
|
||||||
|
|
||||||
|
bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
|
||||||
|
|
||||||
|
if (bytesProcessed > 0) {
|
||||||
|
unpackResult = IRIS_STM32_Protocol_Unpack(receiveBuffer, bytesProcessed, &command, responseBuffer);
|
||||||
|
|
||||||
|
if (unpackResult >= 0) {
|
||||||
|
// 根据命令类型处理数据
|
||||||
|
switch (command) {
|
||||||
|
case 0x00: { // JSON数据
|
||||||
|
char jsonString[JSON_BUFFER_SIZE];
|
||||||
|
memcpy(jsonString, responseBuffer, unpackResult);
|
||||||
|
jsonString[unpackResult] = '\0'; // 确保字符串以null结尾
|
||||||
|
|
||||||
|
DebugSerial.print("Received JSON: ");
|
||||||
|
DebugSerial.println(jsonString);
|
||||||
|
|
||||||
|
processJsonCommand(jsonString);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case 0x01: { // 字符串数据
|
||||||
|
char stringData[BUFFER_SIZE];
|
||||||
|
memcpy(stringData, responseBuffer, unpackResult);
|
||||||
|
stringData[unpackResult] = '\0';
|
||||||
|
|
||||||
|
DebugSerial.print("Received String: ");
|
||||||
|
DebugSerial.println(stringData);
|
||||||
|
|
||||||
|
processStringCommand(stringData);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
default: { // 自定义二进制数据 (0x02-0xff)
|
||||||
|
DebugSerial.print("Received Binary Data, Command: 0x");
|
||||||
|
DebugSerial.print(command, HEX);
|
||||||
|
DebugSerial.print(", Length: ");
|
||||||
|
DebugSerial.println(unpackResult);
|
||||||
|
|
||||||
|
// 回显
|
||||||
|
packedLength = IRIS_Protocol_Pack(command, unpackResult, responseBuffer, sendBuffer);
|
||||||
|
DebugSerial.write(sendBuffer, packedLength);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
DebugSerial.print("Unpack failed, error code: ");
|
||||||
|
DebugSerial.println(unpackResult);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void processJsonCommand(const char* jsonString) {
|
||||||
|
StaticJsonDocument<JSON_BUFFER_SIZE> doc;
|
||||||
|
DeserializationError error = deserializeJson(doc, jsonString);
|
||||||
|
|
||||||
|
if (error) {
|
||||||
|
DebugSerial.print("JSON parsing failed: ");
|
||||||
|
DebugSerial.println(error.c_str());
|
||||||
|
sendJsonResponse("error", "JSON_PARSE_ERROR");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* cmd = doc["cmd"];
|
||||||
|
if (cmd == nullptr) {
|
||||||
|
sendJsonResponse("error", "NO_CMD_FIELD");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 根据cmd执行相应操作
|
||||||
|
if (strcmp(cmd, "cfg") == 0) {
|
||||||
|
JsonObject configObj = doc.as<JsonObject>();
|
||||||
|
handleConfigCommand(configObj);
|
||||||
|
} else if (strcmp(cmd, "ena") == 0) {
|
||||||
|
handleEnableCommand();
|
||||||
|
} else if (strcmp(cmd, "off") == 0) {
|
||||||
|
handleOffCommand();
|
||||||
|
} else if (strcmp(cmd, "mov") == 0) {
|
||||||
|
handleMoveCommand();
|
||||||
|
} else if (strcmp(cmd, "pos") == 0) {
|
||||||
|
int value = doc["value"] | 0;
|
||||||
|
handlePositionCommand(value);
|
||||||
|
} else if (strcmp(cmd, "rmv") == 0) {
|
||||||
|
int value = doc["value"] | 0;
|
||||||
|
handleRelativeMoveCommand(value);
|
||||||
|
} else if (strcmp(cmd, "pps") == 0) {
|
||||||
|
if (doc.containsKey("value")) {
|
||||||
|
int value = doc["value"];
|
||||||
|
handlePulsePerSecondCommand(value);
|
||||||
|
} else {
|
||||||
|
handlePulsePerSecondCommand();
|
||||||
|
}
|
||||||
|
} else if (strcmp(cmd, "org") == 0) {
|
||||||
|
handleOriginCommand();
|
||||||
|
} else if (strcmp(cmd, "stp") == 0) {
|
||||||
|
handleStopCommand();
|
||||||
|
} else if (strcmp(cmd, "zero") == 0) {
|
||||||
|
const char* value = doc["value"] | "";
|
||||||
|
handleZeroCommand(value);
|
||||||
|
} else {
|
||||||
|
sendJsonResponse(cmd, "UNKNOWN_COMMAND");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void processStringCommand(const char* stringData) {
|
||||||
|
// 处理字符串命令
|
||||||
|
String response = "Echo: " + String(stringData);
|
||||||
|
sendStringResponse(response.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendJsonResponse(const char* cmd, const char* status, int value) {
|
||||||
|
StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
|
||||||
|
responseDoc["cmd"] = cmd;
|
||||||
|
responseDoc["status"] = status;
|
||||||
|
|
||||||
|
if (value != -1) {
|
||||||
|
responseDoc["value"] = value;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 设备状态信息
|
||||||
|
responseDoc["enabled"] = deviceEnabled;
|
||||||
|
responseDoc["position"] = currentPosition;
|
||||||
|
responseDoc["moving"] = isMoving;
|
||||||
|
|
||||||
|
char jsonResponse[JSON_BUFFER_SIZE];
|
||||||
|
serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
|
||||||
|
|
||||||
|
// 打包并发送JSON响应
|
||||||
|
packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
|
||||||
|
DebugSerial.write(sendBuffer, packedLength);
|
||||||
|
|
||||||
|
DebugSerial.println();
|
||||||
|
|
||||||
|
DebugSerial.print("Sent JSON Response: ");
|
||||||
|
DebugSerial.println(jsonResponse);
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendStringResponse(const char* response) {
|
||||||
|
// 打包并发送字符串响应
|
||||||
|
packedLength = IRIS_Protocol_Pack(0x01, strlen(response), (uint8_t*)response, sendBuffer);
|
||||||
|
DebugSerial.write(sendBuffer, packedLength);
|
||||||
|
|
||||||
|
DebugSerial.print("Sent String Response: ");
|
||||||
|
DebugSerial.println(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleConfigCommand(JsonObject& doc) {
|
||||||
|
DebugSerial.println("Processing configuration command...");
|
||||||
|
|
||||||
|
// 创建响应JSON对象
|
||||||
|
StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
|
||||||
|
responseDoc["cmd"] = "cfg";
|
||||||
|
responseDoc["status"] = "OK";
|
||||||
|
|
||||||
|
// 添加设备状态信息
|
||||||
|
responseDoc["enabled"] = deviceEnabled;
|
||||||
|
responseDoc["position"] = currentPosition;
|
||||||
|
responseDoc["moving"] = isMoving;
|
||||||
|
|
||||||
|
// 创建配置参数对象
|
||||||
|
JsonObject configParams = responseDoc.createNestedObject("config");
|
||||||
|
|
||||||
|
// 处理配置参数
|
||||||
|
if (doc.containsKey("params")) {
|
||||||
|
JsonObject params = doc["params"];
|
||||||
|
|
||||||
|
// 更新配置参数并添加到响应中
|
||||||
|
if (params.containsKey("cfg zmd")) {
|
||||||
|
deviceConfig.zmd = params["cfg zmd"];
|
||||||
|
configParams["cfg zmd"] = deviceConfig.zmd;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg snr")) {
|
||||||
|
deviceConfig.snr = params["cfg snr"];
|
||||||
|
configParams["cfg snr"] = deviceConfig.snr;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg osv")) {
|
||||||
|
deviceConfig.osv = params["cfg osv"];
|
||||||
|
configParams["cfg osv"] = deviceConfig.osv;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg zsd")) {
|
||||||
|
deviceConfig.zsd = params["cfg zsd"];
|
||||||
|
configParams["cfg zsd"] = deviceConfig.zsd;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg zsp")) {
|
||||||
|
deviceConfig.zsp = params["cfg zsp"];
|
||||||
|
configParams["cfg zsp"] = deviceConfig.zsp;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg dmd")) {
|
||||||
|
deviceConfig.dmd = params["cfg dmd"];
|
||||||
|
configParams["cfg dmd"] = deviceConfig.dmd;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg dar")) {
|
||||||
|
deviceConfig.dar = params["cfg dar"];
|
||||||
|
configParams["cfg dar"] = deviceConfig.dar;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg msr")) {
|
||||||
|
deviceConfig.msr = params["cfg msr"];
|
||||||
|
configParams["cfg msr"] = deviceConfig.msr;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg msv")) {
|
||||||
|
deviceConfig.msv = params["cfg msv"];
|
||||||
|
configParams["cfg msv"] = deviceConfig.msv;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg psr")) {
|
||||||
|
deviceConfig.psr = params["cfg psr"];
|
||||||
|
configParams["cfg psr"] = deviceConfig.psr;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg psv")) {
|
||||||
|
deviceConfig.psv = params["cfg psv"];
|
||||||
|
configParams["cfg psv"] = deviceConfig.psv;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg bdr")) {
|
||||||
|
deviceConfig.bdr = params["cfg bdr"];
|
||||||
|
configParams["cfg bdr"] = deviceConfig.bdr;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg cid")) {
|
||||||
|
deviceConfig.cid = params["cfg cid"];
|
||||||
|
configParams["cfg cid"] = deviceConfig.cid;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg mcs")) {
|
||||||
|
deviceConfig.mcs = params["cfg mcs"];
|
||||||
|
configParams["cfg mcs"] = deviceConfig.mcs;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg spd")) {
|
||||||
|
deviceConfig.spd = params["cfg spd"];
|
||||||
|
configParams["cfg spd"] = deviceConfig.spd;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg acc")) {
|
||||||
|
deviceConfig.acc = params["cfg acc"];
|
||||||
|
configParams["cfg acc"] = deviceConfig.acc;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg dec")) {
|
||||||
|
deviceConfig.dec = params["cfg dec"];
|
||||||
|
configParams["cfg dec"] = deviceConfig.dec;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg cra")) {
|
||||||
|
deviceConfig.cra = params["cfg cra"];
|
||||||
|
configParams["cfg cra"] = deviceConfig.cra;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg crn")) {
|
||||||
|
deviceConfig.crn = params["cfg crn"];
|
||||||
|
configParams["cfg crn"] = deviceConfig.crn;
|
||||||
|
}
|
||||||
|
if (params.containsKey("cfg crh")) {
|
||||||
|
deviceConfig.crh = params["cfg crh"];
|
||||||
|
configParams["cfg crh"] = deviceConfig.crh;
|
||||||
|
}
|
||||||
|
|
||||||
|
DebugSerial.println("Configuration updated successfully");
|
||||||
|
} else {
|
||||||
|
// 如果没有params，返回所有当前配置
|
||||||
|
configParams["cfg zmd"] = deviceConfig.zmd;
|
||||||
|
configParams["cfg snr"] = deviceConfig.snr;
|
||||||
|
configParams["cfg osv"] = deviceConfig.osv;
|
||||||
|
configParams["cfg zsd"] = deviceConfig.zsd;
|
||||||
|
configParams["cfg zsp"] = deviceConfig.zsp;
|
||||||
|
configParams["cfg dmd"] = deviceConfig.dmd;
|
||||||
|
configParams["cfg dar"] = deviceConfig.dar;
|
||||||
|
configParams["cfg msr"] = deviceConfig.msr;
|
||||||
|
configParams["cfg msv"] = deviceConfig.msv;
|
||||||
|
configParams["cfg psr"] = deviceConfig.psr;
|
||||||
|
configParams["cfg psv"] = deviceConfig.psv;
|
||||||
|
configParams["cfg bdr"] = deviceConfig.bdr;
|
||||||
|
configParams["cfg cid"] = deviceConfig.cid;
|
||||||
|
configParams["cfg mcs"] = deviceConfig.mcs;
|
||||||
|
configParams["cfg spd"] = deviceConfig.spd;
|
||||||
|
configParams["cfg acc"] = deviceConfig.acc;
|
||||||
|
configParams["cfg dec"] = deviceConfig.dec;
|
||||||
|
configParams["cfg cra"] = deviceConfig.cra;
|
||||||
|
configParams["cfg crn"] = deviceConfig.crn;
|
||||||
|
configParams["cfg crh"] = deviceConfig.crh;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 序列化并发送响应
|
||||||
|
char jsonResponse[JSON_BUFFER_SIZE];
|
||||||
|
serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
|
||||||
|
|
||||||
|
// 打包并发送JSON响应
|
||||||
|
packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
|
||||||
|
DebugSerial.write(sendBuffer, packedLength);
|
||||||
|
|
||||||
|
DebugSerial.print("Sent Config Response: ");
|
||||||
|
DebugSerial.println(jsonResponse);
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleEnableCommand() {
|
||||||
|
deviceEnabled = true;
|
||||||
|
DebugSerial.println("Device enabled");
|
||||||
|
Serial.print("ena\n");
|
||||||
|
sendJsonResponse("ena", "OK");
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleOffCommand() {
|
||||||
|
deviceEnabled = false;
|
||||||
|
isMoving = false;
|
||||||
|
DebugSerial.println("Device disabled");
|
||||||
|
Serial.print("off\n");
|
||||||
|
sendJsonResponse("off", "OK");
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleMoveCommand() {
|
||||||
|
if (!deviceEnabled) {
|
||||||
|
sendJsonResponse("mov", "DEVICE_DISABLED");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
isMoving = true;
|
||||||
|
DebugSerial.println("Starting movement");
|
||||||
|
Serial.print("mov\n");
|
||||||
|
sendJsonResponse("mov", "OK");
|
||||||
|
}
|
||||||
|
|
||||||
|
void handlePositionCommand(int value) {
|
||||||
|
if (!deviceEnabled) {
|
||||||
|
sendJsonResponse("pos", "DEVICE_DISABLED");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
targetPosition = value;
|
||||||
|
currentPosition = value; // 模拟立即到达
|
||||||
|
DebugSerial.print("Position set to: ");
|
||||||
|
DebugSerial.println(value);
|
||||||
|
sendJsonResponse("pos", "OK", value);
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleRelativeMoveCommand(int value) {
|
||||||
|
if (!deviceEnabled) {
|
||||||
|
sendJsonResponse("rmv", "DEVICE_DISABLED");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
currentPosition += value;
|
||||||
|
DebugSerial.print("Relative move by: ");
|
||||||
|
DebugSerial.print(value);
|
||||||
|
DebugSerial.print(", new position: ");
|
||||||
|
DebugSerial.println(currentPosition);
|
||||||
|
sendJsonResponse("rmv", "OK", currentPosition);
|
||||||
|
}
|
||||||
|
|
||||||
|
void handlePulsePerSecondCommand(int value) {
|
||||||
|
if (value == -1) {
|
||||||
|
// SET PPS
|
||||||
|
Serial.print("pps\n");
|
||||||
|
sendJsonResponse("pps", "OK", pulsePerSecond);
|
||||||
|
} else {
|
||||||
|
// 设置PPS值
|
||||||
|
pulsePerSecond = value;
|
||||||
|
DebugSerial.print("Pulse per second set to: ");
|
||||||
|
DebugSerial.println(value);
|
||||||
|
sendJsonResponse("pps", "OK", value);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleOriginCommand() {
|
||||||
|
if (!deviceEnabled) {
|
||||||
|
sendJsonResponse("org", "DEVICE_DISABLED");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
currentPosition = 0;
|
||||||
|
targetPosition = 0;
|
||||||
|
DebugSerial.println("Returned to origin");
|
||||||
|
Serial.print("org\n");
|
||||||
|
sendJsonResponse("org", "OK", 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleStopCommand() {
|
||||||
|
isMoving = false;
|
||||||
|
DebugSerial.println("Movement stopped");
|
||||||
|
Serial.print("stp\n");
|
||||||
|
sendJsonResponse("stp", "OK");
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleZeroCommand(const char* value) {
|
||||||
|
if (strcmp(value, "start") == 0) {
|
||||||
|
Serial.print("zero start\n");
|
||||||
|
isBackZero = true;
|
||||||
|
DebugSerial.println("Zero started");
|
||||||
|
sendJsonResponse("zero", "ZERO_STARTED");
|
||||||
|
} else if (strcmp(value, "stop") == 0) {
|
||||||
|
Serial.print("zero stop\n");
|
||||||
|
isBackZero = false;
|
||||||
|
currentPosition = 0;
|
||||||
|
DebugSerial.println("Zero completed");
|
||||||
|
sendJsonResponse("zero", "ZERO_COMPLETED");
|
||||||
|
} else {
|
||||||
|
sendJsonResponse("zero", "INVALID_VALUE");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void printDeviceStatus() {
|
||||||
|
DebugSerial.println("\n=== Device Status ===");
|
||||||
|
DebugSerial.print("Enabled: ");
|
||||||
|
DebugSerial.println(deviceEnabled ? "Yes" : "No");
|
||||||
|
DebugSerial.print("Current Position: ");
|
||||||
|
DebugSerial.println(currentPosition);
|
||||||
|
DebugSerial.print("Target Position: ");
|
||||||
|
DebugSerial.println(targetPosition);
|
||||||
|
DebugSerial.print("Pulse Per Second: ");
|
||||||
|
DebugSerial.println(pulsePerSecond);
|
||||||
|
DebugSerial.print("Moving: ");
|
||||||
|
DebugSerial.println(isMoving ? "Yes" : "No");
|
||||||
|
DebugSerial.print("Zero Backing: ");
|
||||||
|
DebugSerial.println(isBackZero ? "Yes" : "No");
|
||||||
|
DebugSerial.println("==================\n");
|
||||||
|
}
|
||||||
272
src/vsmd_parser.cpp
Normal file
272
src/vsmd_parser.cpp
Normal file
@ -0,0 +1,272 @@
|
|||||||
|
#include "vsmd_parser.h"
|
||||||
|
#include "ble.h"
|
||||||
|
|
||||||
|
extern HardwareSerial debugSerial;
|
||||||
|
|
||||||
|
// BCC 校验函数
|
||||||
|
uint8_t VSMDParser::bcc_checksum(uint8_t* data, int size) {
|
||||||
|
uint8_t sum = 0;
|
||||||
|
for (int index = 0; index < size; index++) {
|
||||||
|
sum ^= data[index];
|
||||||
|
}
|
||||||
|
return sum;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 构造函数
|
||||||
|
VSMDParser::VSMDParser() {
|
||||||
|
// 可选初始化
|
||||||
|
}
|
||||||
|
|
||||||
|
// 数据转换函数
|
||||||
|
void VSMDParser::convert(uint8_t* data, value_info& info) {
|
||||||
|
info.udata = data[0];
|
||||||
|
info.udata <<= 7;
|
||||||
|
info.udata |= data[1];
|
||||||
|
info.udata <<= 7;
|
||||||
|
info.udata |= data[2];
|
||||||
|
info.udata <<= 7;
|
||||||
|
info.udata |= data[3];
|
||||||
|
info.udata <<= 7;
|
||||||
|
info.udata |= data[4];
|
||||||
|
}
|
||||||
|
|
||||||
|
// 解析函数
|
||||||
|
// void VSMDParser::parse(uint8_t* data, int size) {
|
||||||
|
// // if (size < 10) {
|
||||||
|
// // debugSerial.println("data too short");
|
||||||
|
// // return;
|
||||||
|
// // }
|
||||||
|
|
||||||
|
// if (data[0] == 0xFF && data[size - 1] == 0xFE) {
|
||||||
|
// uint8_t checksum = bcc_checksum(&data[1], size - 3);
|
||||||
|
// if (checksum == data[size - 2]) {
|
||||||
|
// switch (data[2]) {
|
||||||
|
// case 1: { // dev
|
||||||
|
// debugSerial.print("dev:");
|
||||||
|
// int start = 0;
|
||||||
|
// while (start < size && data[start] != 'V') start++;
|
||||||
|
// if (start < size) {
|
||||||
|
// int end = start;
|
||||||
|
// while (end < size && data[end] >= 32 && data[end] < 127) end++;
|
||||||
|
// for (int i = start; i < end; i++) {
|
||||||
|
// debugSerial.print((char)data[i]);
|
||||||
|
// }
|
||||||
|
// debugSerial.println();
|
||||||
|
// } else {
|
||||||
|
// debugSerial.println("dev err");
|
||||||
|
// }
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case 2: { // sts
|
||||||
|
// float spd = 0;
|
||||||
|
// int pos = 0;
|
||||||
|
// uint32_t sts = 0;
|
||||||
|
// value_info info;
|
||||||
|
|
||||||
|
// convert(&data[3], info);
|
||||||
|
// spd = info.fdata;
|
||||||
|
|
||||||
|
// convert(&data[8], info);
|
||||||
|
// pos = info.idata;
|
||||||
|
|
||||||
|
// convert(&data[13], info);
|
||||||
|
// sts = info.udata;
|
||||||
|
|
||||||
|
// debugSerial.print("Speed: ");
|
||||||
|
// debugSerial.print(spd);
|
||||||
|
// debugSerial.print(", Position: ");
|
||||||
|
// debugSerial.print(pos);
|
||||||
|
// debugSerial.print(", Status: ");
|
||||||
|
// debugSerial.println(sts);
|
||||||
|
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case 3: { // cfg
|
||||||
|
// debugSerial.print("cfg:");
|
||||||
|
// int payloadStart = 3;
|
||||||
|
// int payloadEnd = size - 3;
|
||||||
|
|
||||||
|
// if (payloadStart < payloadEnd) {
|
||||||
|
// for (int i = payloadStart; i < payloadEnd; i++) {
|
||||||
|
// debugSerial.print((char)data[i]);
|
||||||
|
// }
|
||||||
|
// debugSerial.println();
|
||||||
|
// } else {
|
||||||
|
// debugSerial.println(" 数据长度不足");
|
||||||
|
// }
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case 4: { // demo
|
||||||
|
// debugSerial.print("demo:");
|
||||||
|
// int payloadStart = 3;
|
||||||
|
// int payloadEnd = size - 3;
|
||||||
|
|
||||||
|
// if (payloadStart < payloadEnd) {
|
||||||
|
// for (int i = payloadStart; i < payloadEnd; i++) {
|
||||||
|
// debugSerial.print((char)data[i]);
|
||||||
|
// }
|
||||||
|
// debugSerial.println();
|
||||||
|
// } else {
|
||||||
|
// debugSerial.println(" 数据长度不足");
|
||||||
|
// }
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// default:
|
||||||
|
// debugSerial.print("未知命令: ");
|
||||||
|
// debugSerial.println(data[2]);
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// } else {
|
||||||
|
// return;
|
||||||
|
// debugSerial.println("BCC ERROR");
|
||||||
|
// debugSerial.print("CHECK BCC: ");
|
||||||
|
// debugSerial.println(checksum);
|
||||||
|
// debugSerial.print("RES BCC: ");
|
||||||
|
// debugSerial.println(data[size - 2], HEX);
|
||||||
|
// }
|
||||||
|
// } else {
|
||||||
|
// debugSerial.println("Head or Tail ERROR");
|
||||||
|
// debugSerial.print("Receive Data: ");
|
||||||
|
// for (int i = 0; i < size; i++) {
|
||||||
|
// debugSerial.print(data[i], HEX);
|
||||||
|
// debugSerial.print(" ");
|
||||||
|
// }
|
||||||
|
// debugSerial.println();
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
/**************************** ble read ************************************ */
|
||||||
|
void VSMDParser::parse(uint8_t* data, int size) {
|
||||||
|
if (data[0] == 0xFF && data[size - 1] == 0xFE) {
|
||||||
|
uint8_t checksum = bcc_checksum(&data[1], size - 3);
|
||||||
|
if (checksum == data[size - 2]) {
|
||||||
|
switch (data[2]) {
|
||||||
|
case 1: { // dev
|
||||||
|
String response = "dev:";
|
||||||
|
int start = 0;
|
||||||
|
while (start < size && data[start] != 'V') start++;
|
||||||
|
if (start < size) {
|
||||||
|
int end = start;
|
||||||
|
while (end < size && data[end] >= 32 && data[end] < 127) end++;
|
||||||
|
for (int i = start; i < end; i++) {
|
||||||
|
response += (char)data[i];
|
||||||
|
}
|
||||||
|
response += "\n";
|
||||||
|
} else {
|
||||||
|
response += "dev err\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 2: { // sts
|
||||||
|
float spd = 0;
|
||||||
|
int pos = 0;
|
||||||
|
uint32_t sts = 0;
|
||||||
|
value_info info;
|
||||||
|
|
||||||
|
convert(&data[3], info);
|
||||||
|
spd = info.fdata;
|
||||||
|
|
||||||
|
convert(&data[8], info);
|
||||||
|
pos = info.idata;
|
||||||
|
|
||||||
|
convert(&data[13], info);
|
||||||
|
sts = info.udata;
|
||||||
|
|
||||||
|
String response = "Speed: " + String(spd) + ", Position: " + String(pos) + ", Status: " + String(sts) + "\n";
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 3: { // cfg
|
||||||
|
String response = "cfg:";
|
||||||
|
int payloadStart = 3;
|
||||||
|
int payloadEnd = size - 3;
|
||||||
|
|
||||||
|
if (payloadStart < payloadEnd) {
|
||||||
|
for (int i = payloadStart; i < payloadEnd; i++) {
|
||||||
|
response += (char)data[i];
|
||||||
|
}
|
||||||
|
response += "\n";
|
||||||
|
} else {
|
||||||
|
response += " 数据长度不足\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case 4: { // demo
|
||||||
|
String response = "demo:";
|
||||||
|
int payloadStart = 3;
|
||||||
|
int payloadEnd = size - 3;
|
||||||
|
|
||||||
|
if (payloadStart < payloadEnd) {
|
||||||
|
for (int i = payloadStart; i < payloadEnd; i++) {
|
||||||
|
response += (char)data[i];
|
||||||
|
}
|
||||||
|
response += "\n";
|
||||||
|
} else {
|
||||||
|
response += " 数据长度不足\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
String response = "未知命令: ";
|
||||||
|
response += data[2];
|
||||||
|
response += "\n";
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// BCC 校验失败
|
||||||
|
String response = "BCC ERROR\nCHECK BCC: ";
|
||||||
|
response += checksum;
|
||||||
|
response += "\nRES BCC: ";
|
||||||
|
response += data[size - 2], HEX;
|
||||||
|
response += "\n";
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
String response = "Head or Tail ERROR\nReceive Data: ";
|
||||||
|
for (int i = 0; i < size; i++) {
|
||||||
|
response += data[i], HEX;
|
||||||
|
response += " ";
|
||||||
|
}
|
||||||
|
response += "\n";
|
||||||
|
|
||||||
|
// 发送通过蓝牙
|
||||||
|
if (pCharacteristic) {
|
||||||
|
pCharacteristic->setValue(response.c_str());
|
||||||
|
pCharacteristic->notify();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
34
src/vsmd_parser.h
Normal file
34
src/vsmd_parser.h
Normal file
@ -0,0 +1,34 @@
|
|||||||
|
#ifndef VSMD_PARSER_H
|
||||||
|
#define VSMD_PARSER_H
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include "ble.h"
|
||||||
|
|
||||||
|
|
||||||
|
// 联合体定义：用于数据转换
|
||||||
|
typedef union {
|
||||||
|
float fdata;
|
||||||
|
int32_t idata;
|
||||||
|
uint32_t udata;
|
||||||
|
} value_info;
|
||||||
|
|
||||||
|
// 解析类声明
|
||||||
|
class VSMDParser {
|
||||||
|
public:
|
||||||
|
VSMDParser();
|
||||||
|
|
||||||
|
void parse(uint8_t* data, int size);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// BCC校验函数
|
||||||
|
static uint8_t bcc_checksum(uint8_t* data, int size);
|
||||||
|
|
||||||
|
// 将4字节数据转换为联合体中的值
|
||||||
|
void convert(uint8_t* data, value_info& info);
|
||||||
|
};
|
||||||
|
|
||||||
|
extern VSMDParser vsmdParser;
|
||||||
|
|
||||||
|
extern BLECharacteristic* pCharacteristic; // 外部 BLE 特征值指针声明
|
||||||
|
|
||||||
|
#endif
|
||||||
11
test/README
Normal file
11
test/README
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
|
||||||
|
This directory is intended for PlatformIO Test Runner and project tests.
|
||||||
|
|
||||||
|
Unit Testing is a software testing method by which individual units of
|
||||||
|
source code, sets of one or more MCU program modules together with associated
|
||||||
|
control data, usage procedures, and operating procedures, are tested to
|
||||||
|
determine whether they are fit for use. Unit testing finds problems early
|
||||||
|
in the development cycle.
|
||||||
|
|
||||||
|
More information about PlatformIO Unit Testing:
|
||||||
|
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
|
||||||
23
test5.json
Normal file
23
test5.json
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
{
|
||||||
|
"limit": {
|
||||||
|
"cfg msr": 1, //设置负极限传感器
|
||||||
|
"cfg msv": 0, //设置负极限触发电平
|
||||||
|
"cfg psr": 2, //设置正极限传感器
|
||||||
|
"cfg psv": 0 //设置正极限触发电平
|
||||||
|
},
|
||||||
|
"other": {
|
||||||
|
"cfg acc": 20000, //设置加速度
|
||||||
|
"cfg cra": 4, //设置加速时电流
|
||||||
|
"cfg crh": 1, //设置HOLD电流
|
||||||
|
"cfg crn": 4, //设置匀速时电流
|
||||||
|
"cfg dec": 20000, //设置减速度
|
||||||
|
"cfg mcs": 7 //设置微步细分
|
||||||
|
},
|
||||||
|
"zero start": {
|
||||||
|
"cfg osv": 0, //设置归零传感器OPEN时电平
|
||||||
|
"cfg snr": 0, //设置归零用传感器
|
||||||
|
"cfg zmd": 2, //设置归零功能
|
||||||
|
"cfg zsd": 3000, //设置归零速度（正负代表方向）
|
||||||
|
"cfg zsp": 2400 //设置归零后的安全位置
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user