chenxin/VSMD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first

Browse Source
This commit is contained in:
chenxin
2025-06-27 10:03:05 +08:00
commit 89a0366629
20 changed files with 4520 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View File

File diff suppressed because it is too large Load Diff

19
platformio - 副本.ini Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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 //设置归零后的安全位置
}
}