博客摘录「 六自由度机械臂运动学分析及其轨迹规划」2024年10月8日

间中进行轨迹规划是直接用运动时的受控变量规划轨迹，有着计算

#include <stdio.h>
#include <string.h>
#include <stdint.h>

// 模拟数据结构
typedef struct {
    uint8_t MonitorModbusAdd1[400]; // 30个命令 * 10字节 + 预留
} GenSetMonitorModbusAdd_t;

GenSetMonitorModbusAdd_t GenSetMonitorModbusAdd;

// 模拟函数原型
int GetstrLen(char* str, char delimiter);
int Get_Command_Val(char* src, uint8_t* dest, char delimiter, int* strPos, int* temp_num);

// 模拟测试主函数
void Simulate_Parse_Modbus_Commands()
{
    // 模拟输入数据（来自串口接收缓冲区）
    char RXBufferUsart4[] = "{\"device_id\":\"12345\",\"para_command\":\"01010000004EBC3E;01030000002D85D7;0103002E000AA5C4\",\"con_command\":\"01010000004EBC3E;01030000002D85D7;0103002E000AA5C4\",\"online_rate\":5000}";
    
    char commandStr[2000];
    char* pstr;
    int len, i, temp1;
    int strP = 0, temp_num = 0;
    uint8_t* val;
    int writeModbusAdd_en = 0;
    int countModbusMonitor = 0;
    
    printf("=== Modbus命令解析模拟测试 ===\n");
    printf("原始JSON数据:\n%s\n\n", RXBufferUsart4);
    
    // 1. 查找para_command字段
    pstr = strstr((char *)RXBufferUsart4, "para_command");
    if (NULL != pstr)
    {
        printf("1. 找到para_command字段，位置: %ld\n", pstr - RXBufferUsart4);
        
        // 2. 提取para_command字段值
        len = GetstrLen(pstr, ',');  // 获取到下一个逗号前的长度
        printf("2. para_command值长度: %d\n", len);
        
        memset(commandStr, 0, 2000);
        memcpy(commandStr, pstr + 15, len - 16);  // 跳过 "para_command":"
        printf("3. 提取的命令字符串: \"%s\"\n", commandStr);
        
        // 3. 清空Modbus地址存储区
        memset(&GenSetMonitorModbusAdd.MonitorModbusAdd1[0], 0, 400);
        strP = 0;  // 字符串解析位置指针
        
        printf("4. 开始解析Modbus命令...\n");
        
        // 4. 解析多个Modbus命令（最多30个）
        for (i = 0; i < 30; i++)
        {
            // 每个Modbus命令占用10字节的空间
            val = &GenSetMonitorModbusAdd.MonitorModbusAdd1[0] + 10 * i;
            
            printf("  命令[%d]存储位置: %p\n", i, (void*)val);
            
            // 解析单个Modbus命令
            temp1 = Get_Command_Val(commandStr + strP, val, ';', &strP, &temp_num);
            
            // 如果字符串结束，提前退出循环
            if (commandStr[strP] == '\0') {
                countModbusMonitor = i + 1;
                printf("  字符串结束，实际命令数: %d\n", countModbusMonitor);
                break;
            }
        }
        
        // 5. 设置写入标志
        writeModbusAdd_en = 1;  // 通知系统需要写入配置到存储
        printf("\n5. 解析完成，设置写入标志: writeModbusAdd_en = %d\n", writeModbusAdd_en);
        
        // 6. 显示解析结果
        printf("\n=== 解析结果详情 ===\n");
        printf("命令字符串: %s\n", commandStr);
        printf("实际解析命令数: %d\n", countModbusMonitor);
        printf("每个命令占用: 10字节\n");
        printf("总占用空间: %d字节\n", countModbusMonitor * 10);
        
        // 显示每个命令的存储内容
        printf("\n存储内容（十六进制）:\n");
        for (i = 0; i < countModbusMonitor; i++) {
            printf("命令%d: ", i);
            for (int j = 0; j < 10; j++) {
                printf("%02X ", GenSetMonitorModbusAdd.MonitorModbusAdd1[i*10 + j]);
            }
            printf("\n");
        }
    }
    else
    {
        printf("未找到para_command字段\n");
    }
}

// 模拟实现GetstrLen函数
int GetstrLen(char* str, char delimiter)
{
    int len = 0;
    while (str[len] != delimiter && str[len] != '\0' && str[len] != '}') {
        len++;
    }
    return len;
}

// 模拟实现Get_Command_Val函数
int Get_Command_Val(char* src, uint8_t* dest, char delimiter, int* strPos, int* temp_num)
{
    char hexStr[13] = {0}; // 12字符 + null终止符
    int i = 0;
    
    printf("    解析命令: ");
    
    // 提取直到分隔符或字符串结束
    while (src[i] != delimiter && src[i] != '\0' && i < 12) {
        hexStr[i] = src[i];
        printf("%c", src[i]);
        i++;
    }
    
    printf(" (长度: %d)\n", i);
    
    if (i == 12) { // 标准Modbus RTU命令长度
        // 将十六进制字符串转换为字节
        for (int j = 0; j < 6; j++) {
            char hex[3] = {hexStr[j*2], hexStr[j*2+1], '\0'};
            dest[j] = (uint8_t)strtol(hex, NULL, 16);
        }
        
        // 剩余4字节填充0（预留）
        for (int j = 6; j < 10; j++) {
            dest[j] = 0;
        }
        
        *strPos += i;
        if (src[i] == delimiter) {
            (*strPos)++; // 跳过分号
        }
        
        return 1; // 成功
    }
    
    return 0; // 失败
}

int main()
{
    Simulate_Parse_Modbus_Commands();
    return 0;
}

量