493 lines
17 KiB
C++
493 lines
17 KiB
C++
#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");
|
||
} |