435 lines
14 KiB
C++
435 lines
14 KiB
C++
#include <ArduinoJson.h>
|
||
#include "IRIS_Method.h"
|
||
#include "vsmd_parser.h"
|
||
|
||
VSMDParser parser;
|
||
|
||
#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 handle_cfg_cmd();
|
||
void handle_ena_cmd();
|
||
void handle_off_cmd();
|
||
void handle_mov_cmd();
|
||
void handle_pos_cmd(int value);
|
||
void handle_rmv_cmd(int value);
|
||
void handle_pps_cmd(int value = -1);
|
||
void handle_org_cmd();
|
||
void handle_stp_cmd();
|
||
void handle_zero_cmd(const char* value);
|
||
void handle_dev_cmd();
|
||
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:dev, cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
|
||
printDeviceStatus();
|
||
}
|
||
|
||
void loop() {
|
||
if (DebugSerial.available() > 0) {
|
||
int bytesRead = DebugSerial.readBytes(receiveBuffer, sizeof(receiveBuffer));
|
||
|
||
// // 添加调试信息
|
||
// DebugSerial.print("Bytes read: ");
|
||
// DebugSerial.println(bytesRead);
|
||
// DebugSerial.print("Raw data: ");
|
||
// for(int i = 0; i < bytesRead; i++) {
|
||
// DebugSerial.print(receiveBuffer[i], HEX);
|
||
// DebugSerial.print(" ");
|
||
// }
|
||
// DebugSerial.println();
|
||
|
||
// bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
|
||
|
||
// // 添加更多调试信息
|
||
// DebugSerial.print("Bytes after header cut: ");
|
||
// DebugSerial.println(bytesProcessed);
|
||
|
||
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;
|
||
}
|
||
|
||
// 使用 command 字段替代 cmd
|
||
const char* command = doc["command"];
|
||
if (command == nullptr) {
|
||
sendJsonResponse("error", "NO_COMMAND_FIELD");
|
||
return;
|
||
}
|
||
|
||
// 获取 PA 参数对象（可选）
|
||
JsonObject params = doc["PA"];
|
||
|
||
// 根据command执行相应操作
|
||
if (strcmp(command, "cfg") == 0) {
|
||
handle_cfg_cmd();
|
||
} else if (strcmp(command, "ena") == 0) {
|
||
handle_ena_cmd();
|
||
} else if (strcmp(command, "off") == 0) {
|
||
handle_off_cmd();
|
||
} else if (strcmp(command, "mov") == 0) {
|
||
handle_mov_cmd();
|
||
} else if (strcmp(command, "pos") == 0) {
|
||
// 修正：正确的类型转换
|
||
int value = params.containsKey("value") ? params["value"].as<int>() : 0;
|
||
handle_pos_cmd(value);
|
||
} else if (strcmp(command, "rmv") == 0) {
|
||
int value = params.containsKey("value") ? params["value"].as<int>() : 0;
|
||
handle_rmv_cmd(value);
|
||
} else if (strcmp(command, "pps") == 0) {
|
||
if (params.containsKey("value")) {
|
||
int value = params["value"].as<int>();
|
||
handle_pps_cmd(value);
|
||
} else {
|
||
handle_pps_cmd();
|
||
}
|
||
} else if (strcmp(command, "org") == 0) {
|
||
handle_org_cmd();
|
||
} else if (strcmp(command, "stp") == 0) {
|
||
handle_stp_cmd();
|
||
} else if (strcmp(command, "zero") == 0) {
|
||
// 修正：正确的字符串类型转换
|
||
const char* value = params.containsKey("value") ? params["value"].as<const char*>() : "";
|
||
handle_zero_cmd(value);
|
||
} else if (strcmp(command, "dev") == 0) {
|
||
handle_dev_cmd();
|
||
} else {
|
||
sendJsonResponse(command, "UNKNOWN_COMMAND");
|
||
}
|
||
}
|
||
|
||
void processStringCommand(const char* stringData) {
|
||
// 处理字符串命令
|
||
String response = "Echo: " + String(stringData);
|
||
sendStringResponse(response.c_str());
|
||
}
|
||
|
||
void sendJsonResponse(const char* command, const char* status, int value) {
|
||
StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
|
||
|
||
// 修改：使用新的JSON格式
|
||
responseDoc["command"] = command; // 使用 command 替代 cmd
|
||
responseDoc["ST"] = status; // 使用 ST 替代 status
|
||
|
||
// 如果有参数值，添加到 PA 对象中
|
||
if (value != -1) {
|
||
JsonObject params = responseDoc.createNestedObject("PA");
|
||
params["value"] = value;
|
||
}
|
||
|
||
// 可选：添加其他信息
|
||
responseDoc["Other"] = "";
|
||
|
||
char jsonResponse[JSON_BUFFER_SIZE];
|
||
serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
|
||
|
||
// 打包并发送JSON响应
|
||
DebugSerial.print("JSON Response: ");
|
||
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 handle_cfg_cmd() {
|
||
DebugSerial.println("Configuration requested");
|
||
Serial.println("cfg\n");
|
||
// 返回配置相关信息
|
||
sendJsonResponse("cfg", "OK");
|
||
}
|
||
|
||
// void handle_dev_cmd() {
|
||
// DebugSerial.println("dev_requested");
|
||
// Serial.println("dev\n");
|
||
|
||
// //test VSMD数据
|
||
// uint8_t testData[] = {
|
||
// 0xFF, 0x00, 0x01, 0x56, 0x53, 0x4D, 0x44, 0x31, 0x34, 0x32, 0x2D, 0x30, 0x32, 0x35, 0x54, 0x2D, 0x31, 0x2E, 0x31, 0x2E, 0x30, 0x32, 0x30, 0x2E, 0x32, 0x31, 0x31, 0x31, 0x31, 0x35, 0x00, 0x42, 0xFE
|
||
// };
|
||
// // 解析
|
||
// DebugSerial.println("Parsing VSMD test data:");
|
||
// parser.parse(testData, sizeof(testData));
|
||
// // 返回设备相关信息
|
||
// sendJsonResponse("dev", "OK");
|
||
// }
|
||
void handle_dev_cmd() {
|
||
DebugSerial.println("dev_requested");
|
||
Serial.println("dev\n");
|
||
|
||
// vsmd test data
|
||
uint8_t testData[] = {
|
||
0xFF, 0x00, 0x01, 0x56, 0x53, 0x4D, 0x44, 0x31, 0x34, 0x32, 0x2D, 0x30, 0x32, 0x35, 0x54, 0x2D,0x31, 0x2E, 0x31, 0x2E, 0x30, 0x32, 0x30, 0x2E, 0x32, 0x31, 0x31, 0x31, 0x31, 0x35, 0x00, 0x42, 0xFE
|
||
};
|
||
|
||
// parseInfo获取解析结果
|
||
String deviceInfo = parser.parseInfo(testData, sizeof(testData));
|
||
|
||
// 创建带有解析数据的JSON响应
|
||
StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
|
||
responseDoc["command"] = "dev";
|
||
responseDoc["ST"] = "OK";
|
||
responseDoc["Other"] = deviceInfo;
|
||
|
||
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 handle_ena_cmd() {
|
||
deviceEnabled = true;
|
||
DebugSerial.println("Device enabled");
|
||
Serial.print("ena\n");
|
||
sendJsonResponse("ena", "OK");
|
||
}
|
||
|
||
void handle_off_cmd() {
|
||
deviceEnabled = false;
|
||
isMoving = false;
|
||
DebugSerial.println("Device disabled");
|
||
Serial.print("off\n");
|
||
sendJsonResponse("off", "OK");
|
||
}
|
||
|
||
void handle_mov_cmd() {
|
||
if (!deviceEnabled) {
|
||
sendJsonResponse("mov", "DEVICE_DISABLED");
|
||
return;
|
||
}
|
||
|
||
isMoving = true;
|
||
DebugSerial.println("Starting movement");
|
||
Serial.print("mov\n");
|
||
sendJsonResponse("mov", "OK");
|
||
}
|
||
|
||
void handle_pos_cmd(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 handle_rmv_cmd(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 handle_pps_cmd(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 handle_org_cmd() {
|
||
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 handle_stp_cmd() {
|
||
isMoving = false;
|
||
DebugSerial.println("Movement stopped");
|
||
Serial.print("stp\n");
|
||
sendJsonResponse("stp", "OK");
|
||
}
|
||
|
||
void handle_zero_cmd(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");
|
||
} |