diff --git a/.claude/skills/curtis-motor-protocol.md b/.claude/skills/curtis-motor-protocol.md new file mode 100644 index 0000000..f82a3a9 --- /dev/null +++ b/.claude/skills/curtis-motor-protocol.md @@ -0,0 +1,1084 @@ +# 柯蒂斯电机控制器 CAN 通讯协议 + +## 概述 +本技能文档提供柯蒂斯（Curtis）1298+1220C 电机控制器 CAN 通讯协议参考信息及完整的通讯实现案例。 + +**协议名称：** Can 通讯协议（1298+1220C） +**版本：** V4.0 +**公司：** 上海诺力智能科技有限公司 + +**控制器组件：** +- **Curtis 1298 软件：** Noblift_PS20V_Agv_9820C_191125_12_0_P209_V403_1.exe +- **Curtis 1220C 参数：** 1220C-2201_20191106.CPF +- **Rema 手柄通讯：** 09-01-14 CAN Protocol Specification + +**功能说明：** +- 车辆行走、升降、转向均通过 CAN 总线指令进行控制 +- 支持手动驾驶和自动驾驶模式切换 +- 与 USB-CAN 接口配套使用（参见 can-protocol 技能） + +--- + +## CAN 总线设置 + +### 基本参数 +``` +波特率：125 kbps +Timing0: 0x03 +Timing1: 0x1C +``` + +### CAN ID 分配 + +**Curtis 控制器发送（MISO）：** +- `0x260`：PDO1_MISO - 状态反馈数据 +- `0x360`：PDO2_MISO - 保留 +- **发送频率：** 20ms ± 5ms + +**AGV 控制器发送（MOSI）：** +- `0x1E5`：PDO1_MOSI - 控制命令（行走、提升） +- `0x2E5`：PDO1_MOSI - 转向角度命令 + +**注意：** REMA CAN 手柄使用 ID `0x1E0` + +### 驾驶模式切换条件 + +**手动驾驶模式：** +- 手柄下的接近开关 `J9/SW_3` 必须闭合（ON）才能有效 + +**自动驾驶模式：** +- `J24/SW_1 = ON` 为自动驾驶模式有效 + +**通讯超时：** +- "AGV 通讯超时"参数可在诺力设置中配置 +- Curtis 监控 AGV 的 CAN 帧是否在线 +- **重要：** 必须以 ≤20ms 周期持续发送控制命令，否则控制器会进入超时保护 + +--- + +## 数据帧格式定义 + +### PDO1_MOSI（AGV → Curtis）- 0x1E5 + +**功能：** AGV 上位机向 Curtis 控制器发送控制命令 + +#### Byte0 - 控制位 1 +| Bit | 功能 | 0 | 1 | +|-----|------|---|---| +| 0 | 调速开关 | 有效 | 无效 | +| 1 | 紧急反向开关 | 无效 | 有效 | +| 2 | 龟速开关 | 无效 | 有效 | +| 3 | 喇叭开关 | 无效 | 有效 | +| 4 | 左侧提升开关 | 无效 | 有效 | +| 5 | 左侧下降开关 | 无效 | 有效 | +| 6 | 右侧提升开关 | 无效 | 有效 | +| 7 | 右侧下降开关 | 无效 | 有效 | + +#### Byte1 - 控制位 2（仅 AGV CAN 发送，REMA 手柄不发送） +| Bit | 功能 | 0 | 1 | +|-----|------|---|---| +| 0 | AGV 标识 | 无效 | AGV 连接 | +| 1 | 紧急停车 | 无效 | 紧停 | +| 2 | 找中完成标记 | - | 完成 | +| 3-7 | 保留 | - | - | + +#### Byte2-3 - 行走调速信号 +- **范围：** -4096 ~ +4095（有符号 16 位） +- **Byte2：** 低 8 位 +- **Byte3：** 高 8 位 + +#### Byte4-5 - 提升/上下倾调速信号（左侧） +- **范围：** -4096 ~ +4095（有符号 16 位） +- **Byte4：** 低 8 位 +- **Byte5：** 高 8 位 + +#### Byte6-7 - 提升调速信号（右侧） +- **范围：** -4096 ~ +4095（有符号 16 位） +- **Byte6：** 低 8 位 +- **Byte7：** 高 8 位 + +--- + +### PDO1_MOSI（AGV → Curtis）- 0x2E5 + +**功能：** 转向角度控制命令 + +#### Byte0-1 - 转向角度要求 +- **范围：** -900 ~ +900（对应 -90° ~ +90°） +- **Byte0：** 低 8 位 +- **Byte1：** 高 8 位 + +**重要配置：** 1220C 参数需设置为： +``` +command device/can/ + can steer left stop to centre = -900 + can steer right stop to centre = 900 +``` + +#### Byte2-7 - 保留 +- 填充 0x00 + +--- + +### PDO1_MISO（Curtis → AGV）- 0x260 + +**功能：** Curtis 控制器向 AGV 上位机反馈状态数据 + +#### Byte0 - 行走故障代码 +- **格式：** 16 进制 +- **说明：** 行走电机相关故障代码 + +#### Byte1 - 转向故障代码 +- **格式：** 16 进制 +- **说明：** 转向电机相关故障代码 + +#### Byte2-3 - 转向角度反馈 +- **范围：** -16383 ~ +16383（对应 -90° ~ +90°） +- **Byte2：** 低位 +- **Byte3：** 高位 + +#### Byte4-5 - 电机转速 +- **范围：** -4096 ~ +4095（区分正反转） +- **Byte4：** 低 8 位 +- **Byte5：** 高 8 位 + +#### Byte6 - 电量 +- **范围：** 0 ~ 100（百分比） + +#### Byte7 - 状态位 +| Bit | 功能 | 0 | 1 | +|-----|------|---|---| +| 0 | 手自动状态 | 手动 | 自动 | +| 1 | 紧急停车 | 无急停 | 急停 | +| 2-7 | 预留 | - | - | + +--- + +### PDO2_MISO（Curtis → AGV）- 0x360 + +**功能：** 保留用途 + +**数据字节定义：** +- Byte0 ~ Byte7：保留，填充 0x00 + +--- + +## 通讯协议摘要表 + +### 上位机向控制器发送（MOSI） + +| 类型 | CAN ID | Data1 | Data2 | Data3 | Data4 | Data5 | Data6 | Data7 | Data8 | +|------|--------|-------|-------|-------|-------|-------|-------|-------|-------| +| MOSI1 | 0x1E5 | 控制位1 | 控制位2 | 行走低位 | 行走高位 | 左提升低位 | 左提升高位 | 右提升低位 | 右提升高位 | +| MOSI2 | 0x2E5 | 角度低位 | 角度高位 | 0x00 | 0x00 | 0x00 | 0x00 | 0x00 | 0x00 | + +### 控制器回复上位机（MISO） + +| 类型 | CAN ID | Data1 | Data2 | Data3 | Data4 | Data5 | Data6 | Data7 | Data8 | +|------|--------|-------|-------|-------|-------|-------|-------|-------|-------| +| MISO1 | 0x260 | 行走故障 | 转向故障 | 角度低位 | 角度高位 | 转速低位 | 转速高位 | 电量 | 状态位 | +| MISO2 | 0x360 | 0x00 | 0x00 | 0x00 | 0x00 | 0x00 | 0x00 | 0x00 | 0x00 | + +--- + +## 完整通讯实现案例 + +### 头文件和数据结构定义 + +```c +// curtis_motor_controller.h +#ifndef CURTIS_MOTOR_CONTROLLER_H +#define CURTIS_MOTOR_CONTROLLER_H + +#include "ControlCAN.h" +#include +#include +#include +#include + +// CAN 设备配置 +#define CAN_DEVICE_TYPE VCI_USBCAN2 // 4 +#define CAN_DEVICE_INDEX 0 // 第一个设备 +#define CAN_CHANNEL 0 // CAN1 通道 + +// Curtis CAN ID 定义 +#define CURTIS_ID_DRIVE_CMD 0x1E5 // 行走/提升命令 +#define CURTIS_ID_STEER_CMD 0x2E5 // 转向命令 +#define CURTIS_ID_STATUS 0x260 // 状态反馈 +#define CURTIS_ID_RESERVED 0x360 // 保留 + +// 数据范围定义 +#define DRIVE_SPEED_MIN -4096 +#define DRIVE_SPEED_MAX 4095 +#define LIFT_SPEED_MIN -4096 +#define LIFT_SPEED_MAX 4095 +#define STEER_ANGLE_MIN -900 // -90度 +#define STEER_ANGLE_MAX 900 // +90度 + +// 控制位掩码定义（Byte0） +#define CTRL_BIT0_SPEED_SWITCH 0x01 // 调速开关（0=有效） +#define CTRL_BIT1_EMERGENCY_REVERSE 0x02 // 紧急反向 +#define CTRL_BIT2_TURTLE_MODE 0x04 // 龟速开关 +#define CTRL_BIT3_HORN 0x08 // 喇叭 +#define CTRL_BIT4_LIFT_LEFT_UP 0x10 // 左侧提升 +#define CTRL_BIT5_LIFT_LEFT_DOWN 0x20 // 左侧下降 +#define CTRL_BIT6_LIFT_RIGHT_UP 0x40 // 右侧提升 +#define CTRL_BIT7_LIFT_RIGHT_DOWN 0x80 // 右侧下降 + +// 控制位掩码定义（Byte1 - AGV专用） +#define CTRL_AGV_CONNECTED 0x01 // AGV连接标识 +#define CTRL_EMERGENCY_STOP 0x02 // 紧急停车 +#define CTRL_CENTERING_DONE 0x04 // 找中完成 + +// 状态位掩码定义（Byte7） +#define STATUS_AUTO_MODE 0x01 // 0=手动, 1=自动 +#define STATUS_EMERGENCY_STOP 0x02 // 0=无急停, 1=急停 + +/** + * Curtis 控制器状态结构 + */ +typedef struct { + uint8_t driveError; // 行走故障代码 + uint8_t steerError; // 转向故障代码 + float currentAngle; // 当前转向角度（度） + int16_t motorSpeed; // 电机转速 + uint8_t batteryLevel; // 电量百分比 0-100 + bool isAutoMode; // 是否自动模式 + bool isEmergencyStop; // 是否急停状态 + uint32_t lastUpdateTime; // 最后更新时间戳（毫秒） +} CurtisStatus; + +/** + * Curtis 控制命令结构 + */ +typedef struct { + int16_t driveSpeed; // 行走速度 -4096~4095 + int16_t liftSpeedLeft; // 左侧提升速度 -4096~4095 + int16_t liftSpeedRight; // 右侧提升速度 -4096~4095 + int16_t steerAngle; // 转向角度 -900~900 + uint8_t controlBits0; // 控制位0 + uint8_t controlBits1; // 控制位1（AGV专用） +} CurtisCommand; + +/** + * Curtis 控制器管理器 + */ +typedef struct { + DWORD deviceType; + DWORD deviceIndex; + DWORD canChannel; + bool isConnected; + CurtisStatus status; + CurtisCommand lastCommand; +} CurtisController; + +// 函数声明 +bool Curtis_Initialize(CurtisController* ctrl); +bool Curtis_Connect(CurtisController* ctrl); +void Curtis_Disconnect(CurtisController* ctrl); +bool Curtis_SendCommand(CurtisController* ctrl, const CurtisCommand* cmd); +bool Curtis_ReceiveStatus(CurtisController* ctrl); +void Curtis_EmergencyStop(CurtisController* ctrl); +int16_t Curtis_SpeedPercentToCAN(float percent); +float Curtis_SpeedCANToPercent(int16_t canValue); +int16_t Curtis_AngleDegreeToCAN(float degrees); +float Curtis_AngleCANToDegree(int16_t canValue); + +#endif // CURTIS_MOTOR_CONTROLLER_H +``` + +--- + +### 实现文件 + +```c +// curtis_motor_controller.c +#include "curtis_motor_controller.h" +#include + +/** + * 初始化 Curtis 控制器结构 + */ +bool Curtis_Initialize(CurtisController* ctrl) { + if (ctrl == NULL) { + return false; + } + + memset(ctrl, 0, sizeof(CurtisController)); + ctrl->deviceType = CAN_DEVICE_TYPE; + ctrl->deviceIndex = CAN_DEVICE_INDEX; + ctrl->canChannel = CAN_CHANNEL; + ctrl->isConnected = false; + + return true; +} + +/** + * 连接到 Curtis 控制器（通过 CAN 总线） + */ +bool Curtis_Connect(CurtisController* ctrl) { + if (ctrl == NULL) { + printf("[Curtis] 错误: 控制器指针为空\n"); + return false; + } + + if (ctrl->isConnected) { + printf("[Curtis] 警告: 已经连接\n"); + return true; + } + + // 步骤1: 打开 CAN 设备 + printf("[Curtis] 正在打开 CAN 设备 (类型=%d, 索引=%d)...\n", + ctrl->deviceType, ctrl->deviceIndex); + + DWORD ret = VCI_OpenDevice(ctrl->deviceType, ctrl->deviceIndex, 0); + if (ret != 1) { + printf("[Curtis] 错误: 打开设备失败 (返回值=%d)\n", ret); + return false; + } + printf("[Curtis] 设备打开成功\n"); + + // 步骤2: 配置 CAN 参数（125kbps） + VCI_INIT_CONFIG config; + config.AccCode = 0x80000008; + config.AccMask = 0xFFFFFFFF; // 接收所有帧 + config.Filter = 1; // 接收标准帧和扩展帧 + config.Timing0 = 0x03; // 125 kbps + config.Timing1 = 0x1C; + config.Mode = 0; // 正常模式 + config.Reserved = 0; + + printf("[Curtis] 正在初始化 CAN (125kbps)...\n"); + ret = VCI_InitCAN(ctrl->deviceType, ctrl->deviceIndex, ctrl->canChannel, &config); + if (ret != 1) { + printf("[Curtis] 错误: 初始化 CAN 失败 (返回值=%d)\n", ret); + VCI_CloseDevice(ctrl->deviceType, ctrl->deviceIndex); + return false; + } + printf("[Curtis] CAN 初始化成功\n"); + + // 步骤3: 启动 CAN 通道 + printf("[Curtis] 正在启动 CAN 通道...\n"); + ret = VCI_StartCAN(ctrl->deviceType, ctrl->deviceIndex, ctrl->canChannel); + if (ret != 1) { + printf("[Curtis] 错误: 启动 CAN 失败 (返回值=%d)\n", ret); + VCI_CloseDevice(ctrl->deviceType, ctrl->deviceIndex); + return false; + } + printf("[Curtis] CAN 通道启动成功\n"); + + // 步骤4: 清空缓冲区 + VCI_ClearBuffer(ctrl->deviceType, ctrl->deviceIndex, ctrl->canChannel); + + ctrl->isConnected = true; + printf("[Curtis] ✓ 连接成功！准备通讯\n"); + + return true; +} + +/** + * 断开与 Curtis 控制器的连接 + */ +void Curtis_Disconnect(CurtisController* ctrl) { + if (ctrl == NULL) { + return; + } + + if (!ctrl->isConnected) { + printf("[Curtis] 警告: 尚未连接\n"); + return; + } + + // 发送停止命令 + printf("[Curtis] 正在发送停止命令...\n"); + CurtisCommand stopCmd = {0}; + stopCmd.controlBits0 = 0x00; // 调速开关有效 + stopCmd.controlBits1 = CTRL_AGV_CONNECTED; // 保持AGV连接标识 + Curtis_SendCommand(ctrl, &stopCmd); + Sleep(50); // 等待命令发送 + + // 关闭设备 + printf("[Curtis] 正在关闭 CAN 设备...\n"); + VCI_CloseDevice(ctrl->deviceType, ctrl->deviceIndex); + + ctrl->isConnected = false; + printf("[Curtis] ✓ 断开连接完成\n"); +} + +/** + * 发送控制命令到 Curtis 控制器 + */ +bool Curtis_SendCommand(CurtisController* ctrl, const CurtisCommand* cmd) { + if (ctrl == NULL || cmd == NULL) { + return false; + } + + if (!ctrl->isConnected) { + printf("[Curtis] 错误: 未连接到控制器\n"); + return false; + } + + // 限制数值范围 + int16_t driveSpeed = cmd->driveSpeed; + if (driveSpeed < DRIVE_SPEED_MIN) driveSpeed = DRIVE_SPEED_MIN; + if (driveSpeed > DRIVE_SPEED_MAX) driveSpeed = DRIVE_SPEED_MAX; + + int16_t liftLeft = cmd->liftSpeedLeft; + if (liftLeft < LIFT_SPEED_MIN) liftLeft = LIFT_SPEED_MIN; + if (liftLeft > LIFT_SPEED_MAX) liftLeft = LIFT_SPEED_MAX; + + int16_t liftRight = cmd->liftSpeedRight; + if (liftRight < LIFT_SPEED_MIN) liftRight = LIFT_SPEED_MIN; + if (liftRight > LIFT_SPEED_MAX) liftRight = LIFT_SPEED_MAX; + + int16_t steerAngle = cmd->steerAngle; + if (steerAngle < STEER_ANGLE_MIN) steerAngle = STEER_ANGLE_MIN; + if (steerAngle > STEER_ANGLE_MAX) steerAngle = STEER_ANGLE_MAX; + + // 发送帧1: 0x1E5 - 行走和提升命令 + VCI_CAN_OBJ frame1; + memset(&frame1, 0, sizeof(VCI_CAN_OBJ)); + frame1.ID = CURTIS_ID_DRIVE_CMD; + frame1.SendType = 0; // 正常发送（自动重试） + frame1.RemoteFlag = 0; // 数据帧 + frame1.ExternFlag = 0; // 标准帧 + frame1.DataLen = 8; + + frame1.Data[0] = cmd->controlBits0; + frame1.Data[1] = cmd->controlBits1 | CTRL_AGV_CONNECTED; // 确保AGV标识位 + frame1.Data[2] = driveSpeed & 0xFF; + frame1.Data[3] = (driveSpeed >> 8) & 0xFF; + frame1.Data[4] = liftLeft & 0xFF; + frame1.Data[5] = (liftLeft >> 8) & 0xFF; + frame1.Data[6] = liftRight & 0xFF; + frame1.Data[7] = (liftRight >> 8) & 0xFF; + + DWORD sent = VCI_Transmit(ctrl->deviceType, ctrl->deviceIndex, + ctrl->canChannel, &frame1, 1); + if (sent != 1) { + printf("[Curtis] 错误: 发送 0x1E5 失败\n"); + return false; + } + + // 发送帧2: 0x2E5 - 转向命令 + VCI_CAN_OBJ frame2; + memset(&frame2, 0, sizeof(VCI_CAN_OBJ)); + frame2.ID = CURTIS_ID_STEER_CMD; + frame2.SendType = 0; + frame2.RemoteFlag = 0; + frame2.ExternFlag = 0; + frame2.DataLen = 8; + + frame2.Data[0] = steerAngle & 0xFF; + frame2.Data[1] = (steerAngle >> 8) & 0xFF; + // Byte2-7 保留，填充0 + + sent = VCI_Transmit(ctrl->deviceType, ctrl->deviceIndex, + ctrl->canChannel, &frame2, 1); + if (sent != 1) { + printf("[Curtis] 错误: 发送 0x2E5 失败\n"); + return false; + } + + // 保存最后发送的命令 + ctrl->lastCommand = *cmd; + + return true; +} + +/** + * 接收 Curtis 控制器状态反馈 + */ +bool Curtis_ReceiveStatus(CurtisController* ctrl) { + if (ctrl == NULL) { + return false; + } + + if (!ctrl->isConnected) { + return false; + } + + VCI_CAN_OBJ rxFrames[2500]; + DWORD count = VCI_Receive(ctrl->deviceType, ctrl->deviceIndex, + ctrl->canChannel, rxFrames, 2500, 0); + + if (count == (DWORD)-1) { + printf("[Curtis] 错误: USB 断开或设备不存在\n"); + ctrl->isConnected = false; + return false; + } + + bool statusUpdated = false; + + for (DWORD i = 0; i < count; i++) { + if (rxFrames[i].ID == CURTIS_ID_STATUS) { + // 解析 0x260 状态帧 + ctrl->status.driveError = rxFrames[i].Data[0]; + ctrl->status.steerError = rxFrames[i].Data[1]; + + int16_t angleRaw = (int16_t)((rxFrames[i].Data[3] << 8) | + rxFrames[i].Data[2]); + ctrl->status.currentAngle = angleRaw / 10.0f; + + ctrl->status.motorSpeed = (int16_t)((rxFrames[i].Data[5] << 8) | + rxFrames[i].Data[4]); + + ctrl->status.batteryLevel = rxFrames[i].Data[6]; + + ctrl->status.isAutoMode = (rxFrames[i].Data[7] & STATUS_AUTO_MODE) != 0; + ctrl->status.isEmergencyStop = (rxFrames[i].Data[7] & STATUS_EMERGENCY_STOP) != 0; + + ctrl->status.lastUpdateTime = GetTickCount(); + statusUpdated = true; + } + else if (rxFrames[i].ID == CURTIS_ID_RESERVED) { + // 0x360 保留帧，暂不处理 + } + } + + return statusUpdated; +} + +/** + * 紧急停车 + */ +void Curtis_EmergencyStop(CurtisController* ctrl) { + if (ctrl == NULL || !ctrl->isConnected) { + return; + } + + printf("[Curtis] !!! 执行紧急停车 !!!\n"); + + CurtisCommand stopCmd = {0}; + stopCmd.controlBits0 = 0x00; // 调速开关有效 + stopCmd.controlBits1 = CTRL_EMERGENCY_STOP | CTRL_AGV_CONNECTED; + stopCmd.driveSpeed = 0; + stopCmd.liftSpeedLeft = 0; + stopCmd.liftSpeedRight = 0; + stopCmd.steerAngle = 0; + + // 连续发送3次确保收到 + for (int i = 0; i < 3; i++) { + Curtis_SendCommand(ctrl, &stopCmd); + Sleep(10); + } +} + +/** + * 速度百分比转 CAN 值 (-100% ~ +100% -> -4096 ~ +4095) + */ +int16_t Curtis_SpeedPercentToCAN(float percent) { + if (percent > 100.0f) percent = 100.0f; + if (percent < -100.0f) percent = -100.0f; + return (int16_t)(percent * 40.95f); +} + +/** + * CAN 值转速度百分比 + */ +float Curtis_SpeedCANToPercent(int16_t canValue) { + return canValue / 40.95f; +} + +/** + * 角度转 CAN 值 (-90° ~ +90° -> -900 ~ +900) + */ +int16_t Curtis_AngleDegreeToCAN(float degrees) { + if (degrees > 90.0f) degrees = 90.0f; + if (degrees < -90.0f) degrees = -90.0f; + return (int16_t)(degrees * 10.0f); +} + +/** + * CAN 值转角度 + */ +float Curtis_AngleCANToDegree(int16_t canValue) { + return canValue / 10.0f; +} +``` + +--- + +### 使用示例 - 主程序 + +```c +// main_curtis_demo.c +#include "curtis_motor_controller.h" +#include +#include + +/** + * 打印 Curtis 状态信息 + */ +void printStatus(const CurtisStatus* status) { + printf("\n========== Curtis 状态 ==========\n"); + printf("行走故障: 0x%02X 转向故障: 0x%02X\n", + status->driveError, status->steerError); + printf("转向角度: %.1f°\n", status->currentAngle); + printf("电机转速: %d\n", status->motorSpeed); + printf("电量: %d%%\n", status->batteryLevel); + printf("模式: %s 急停: %s\n", + status->isAutoMode ? "自动" : "手动", + status->isEmergencyStop ? "是" : "否"); + printf("================================\n\n"); +} + +/** + * 主程序示例：演示完整的 Curtis 控制流程 + */ +int main() { + CurtisController curtis; + bool running = true; + + printf("===== Curtis 电机控制器 CAN 通讯演示 =====\n\n"); + + // ============ 1. 初始化 ============ + printf("步骤 1: 初始化控制器...\n"); + if (!Curtis_Initialize(&curtis)) { + printf("初始化失败！\n"); + return -1; + } + + // ============ 2. 连接 ============ + printf("\n步骤 2: 连接到 Curtis 控制器...\n"); + if (!Curtis_Connect(&curtis)) { + printf("连接失败！\n"); + return -1; + } + + // 等待控制器稳定 + Sleep(100); + + // ============ 3. 主控制循环 ============ + printf("\n步骤 3: 进入主控制循环\n"); + printf("控制说明：\n"); + printf(" W/S - 前进/后退\n"); + printf(" A/D - 左转/右转\n"); + printf(" Q/E - 提升/下降\n"); + printf(" 空格 - 停止\n"); + printf(" ESC - 退出程序\n"); + printf("\n开始控制...\n\n"); + + CurtisCommand cmd = {0}; + cmd.controlBits0 = 0x00; // 调速开关有效 + cmd.controlBits1 = CTRL_AGV_CONNECTED; + + DWORD lastSendTime = GetTickCount(); + DWORD lastPrintTime = GetTickCount(); + int loopCount = 0; + + while (running) { + DWORD currentTime = GetTickCount(); + + // ---- 接收状态（每次循环） ---- + Curtis_ReceiveStatus(&curtis); + + // ---- 键盘控制 ---- + if (_kbhit()) { + char ch = _getch(); + + switch (ch) { + case 'w': case 'W': + cmd.driveSpeed = Curtis_SpeedPercentToCAN(30.0f); // 前进30% + printf("[控制] 前进 30%%\n"); + break; + + case 's': case 'S': + cmd.driveSpeed = Curtis_SpeedPercentToCAN(-30.0f); // 后退30% + printf("[控制] 后退 30%%\n"); + break; + + case 'a': case 'A': + cmd.steerAngle = Curtis_AngleDegreeToCAN(-30.0f); // 左转30度 + printf("[控制] 左转 30°\n"); + break; + + case 'd': case 'D': + cmd.steerAngle = Curtis_AngleDegreeToCAN(30.0f); // 右转30度 + printf("[控制] 右转 30°\n"); + break; + + case 'q': case 'Q': + cmd.liftSpeedLeft = Curtis_SpeedPercentToCAN(50.0f); // 提升 + printf("[控制] 提升\n"); + break; + + case 'e': case 'E': + cmd.liftSpeedLeft = Curtis_SpeedPercentToCAN(-50.0f); // 下降 + printf("[控制] 下降\n"); + break; + + case ' ': // 空格 - 停止 + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + cmd.liftSpeedLeft = 0; + cmd.liftSpeedRight = 0; + printf("[控制] 全部停止\n"); + break; + + case 27: // ESC - 退出 + printf("\n[控制] 用户请求退出\n"); + running = false; + break; + } + } + + // ---- 检查故障和急停 ---- + if (curtis.status.driveError != 0 || curtis.status.steerError != 0) { + printf("[警告] 检测到故障！行走=0x%02X, 转向=0x%02X\n", + curtis.status.driveError, curtis.status.steerError); + } + + if (curtis.status.isEmergencyStop) { + printf("[警告] 急停状态！停止所有运动\n"); + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + cmd.liftSpeedLeft = 0; + cmd.liftSpeedRight = 0; + } + + // ---- 定时发送命令（≤20ms周期） ---- + if (currentTime - lastSendTime >= 15) { // 15ms周期 + if (!Curtis_SendCommand(&curtis, &cmd)) { + printf("[错误] 发送命令失败！\n"); + running = false; + } + lastSendTime = currentTime; + } + + // ---- 定时打印状态（1秒） ---- + if (currentTime - lastPrintTime >= 1000) { + printStatus(&curtis.status); + lastPrintTime = currentTime; + } + + // 避免CPU占用过高 + Sleep(5); + loopCount++; + } + + printf("\n总循环次数: %d\n", loopCount); + + // ============ 4. 断开连接 ============ + printf("\n步骤 4: 断开连接...\n"); + Curtis_Disconnect(&curtis); + + printf("\n===== 程序结束 =====\n"); + return 0; +} +``` + +--- + +### 高级示例 - 路径跟踪控制 + +```c +// path_tracking_demo.c +#include "curtis_motor_controller.h" +#include +#include + +/** + * 路径点结构 + */ +typedef struct { + float x; + float y; + float targetSpeed; // 目标速度百分比 +} PathPoint; + +/** + * 执行路径跟踪控制 + */ +bool executePathTracking(CurtisController* ctrl, const PathPoint* path, int pathLength) { + if (ctrl == NULL || path == NULL || pathLength <= 0) { + return false; + } + + printf("[路径跟踪] 开始执行，共 %d 个路径点\n", pathLength); + + CurtisCommand cmd = {0}; + cmd.controlBits0 = 0x00; + cmd.controlBits1 = CTRL_AGV_CONNECTED; + + for (int i = 0; i < pathLength; i++) { + printf("\n[路径跟踪] 执行路径点 %d/%d: (%.2f, %.2f), 速度=%.1f%%\n", + i + 1, pathLength, path[i].x, path[i].y, path[i].targetSpeed); + + // 设置速度 + cmd.driveSpeed = Curtis_SpeedPercentToCAN(path[i].targetSpeed); + + // 模拟转向角度计算（实际应用中根据路径规划算法计算） + float steerAngleDeg = 0.0f; + if (i + 1 < pathLength) { + float dx = path[i + 1].x - path[i].x; + float dy = path[i + 1].y - path[i].y; + steerAngleDeg = atan2f(dy, dx) * 180.0f / 3.14159f; + // 限制在 ±45 度 + if (steerAngleDeg > 45.0f) steerAngleDeg = 45.0f; + if (steerAngleDeg < -45.0f) steerAngleDeg = -45.0f; + } + cmd.steerAngle = Curtis_AngleDegreeToCAN(steerAngleDeg); + + // 执行该路径点（持续1秒） + DWORD startTime = GetTickCount(); + while (GetTickCount() - startTime < 1000) { + // 接收状态 + Curtis_ReceiveStatus(ctrl); + + // 检查故障 + if (ctrl->status.driveError || ctrl->status.steerError) { + printf("[路径跟踪] 检测到故障，中止路径跟踪\n"); + Curtis_EmergencyStop(ctrl); + return false; + } + + // 检查急停 + if (ctrl->status.isEmergencyStop) { + printf("[路径跟踪] 检测到急停，中止路径跟踪\n"); + return false; + } + + // 发送命令（15ms周期） + Curtis_SendCommand(ctrl, &cmd); + Sleep(15); + } + } + + // 路径结束，停止 + printf("\n[路径跟踪] 路径执行完成，停止车辆\n"); + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + for (int i = 0; i < 5; i++) { + Curtis_SendCommand(ctrl, &cmd); + Sleep(15); + } + + return true; +} + +/** + * 路径跟踪示例主程序 + */ +int main() { + CurtisController curtis; + + printf("===== Curtis 路径跟踪演示 =====\n\n"); + + // 初始化并连接 + if (!Curtis_Initialize(&curtis) || !Curtis_Connect(&curtis)) { + printf("初始化失败！\n"); + return -1; + } + + Sleep(100); + + // 定义测试路径（矩形路径） + PathPoint path[] = { + {0.0f, 0.0f, 30.0f}, // 起点 + {5.0f, 0.0f, 30.0f}, // 直行5米 + {5.0f, 3.0f, 25.0f}, // 右转前进3米 + {0.0f, 3.0f, 25.0f}, // 左转前进5米 + {0.0f, 0.0f, 20.0f} // 返回起点 + }; + int pathLength = sizeof(path) / sizeof(PathPoint); + + // 执行路径跟踪 + bool success = executePathTracking(&curtis, path, pathLength); + + if (success) { + printf("\n✓ 路径跟踪成功完成\n"); + } else { + printf("\n✗ 路径跟踪失败\n"); + } + + // 断开连接 + Curtis_Disconnect(&curtis); + + printf("\n===== 程序结束 =====\n"); + return 0; +} +``` + +--- + +## 错误处理和最佳实践 + +### 错误处理 + +```c +/** + * 检查通讯超时 + */ +bool Curtis_CheckTimeout(CurtisController* ctrl, DWORD timeoutMs) { + if (ctrl == NULL) { + return true; // 视为超时 + } + + DWORD currentTime = GetTickCount(); + DWORD elapsed = currentTime - ctrl->status.lastUpdateTime; + + if (elapsed > timeoutMs) { + printf("[Curtis] 警告: 通讯超时 (%d ms)\n", elapsed); + return true; + } + + return false; +} + +/** + * 通讯恢复处理 + */ +bool Curtis_Reconnect(CurtisController* ctrl) { + printf("[Curtis] 尝试重新连接...\n"); + + Curtis_Disconnect(ctrl); + Sleep(500); + + if (Curtis_Connect(ctrl)) { + printf("[Curtis] 重新连接成功\n"); + return true; + } else { + printf("[Curtis] 重新连接失败\n"); + return false; + } +} +``` + +### 最佳实践清单 + +1. **命令发送频率** + - 必须以 ≤20ms 周期持续发送命令 + - 推荐 10-15ms 周期 + - 超时会导致控制器进入保护模式 + +2. **初始化顺序** + ``` + Curtis_Initialize() → Curtis_Connect() → 主循环 → Curtis_Disconnect() + ``` + +3. **故障处理** + - 实时监控 driveError 和 steerError + - 检测到故障立即停止运动 + - 记录故障代码用于诊断 + +4. **急停处理** + - 检测到急停状态立即停止所有命令 + - 发送停止命令（速度=0） + - 等待急停解除后才能恢复 + +5. **数值范围检查** + - 所有命令值都应进行范围限制 + - 使用提供的转换函数确保数据正确 + +6. **AGV标识位** + - 所有命令必须设置 AGV 连接标识（Byte1 Bit0） + - 否则控制器可能不响应 + +7. **断开连接** + - 程序退出前必须调用 Curtis_Disconnect() + - 发送停止命令后再关闭设备 + +--- + +## 常见问题 + +### 问：无法控制车辆？ +- 检查 CAN 波特率是否为 125kbps（Timing0=0x03, Timing1=0x1C） +- 确认 AGV 标识位已设置（Byte1 Bit0 = 1） +- 手动模式下检查 J9/SW_3 是否闭合 +- 自动模式下检查 J24/SW_1 是否为 ON +- 确认命令发送频率 ≤20ms + +### 问：控制器频繁超时？ +- 检查命令发送周期，必须 ≤20ms +- 检查"AGV 通讯超时"参数设置 +- 验证 CAN 总线连接是否稳定 +- 检查是否有其他设备干扰总线 + +### 问：转向角度不准确？ +- 确认 1220C 参数配置： + - `can steer left stop to centre = -900` + - `can steer right stop to centre = 900` +- 验证角度转换：-900~+900 对应 -90°~+90° +- 检查转向电机是否正常工作 + +### 问：收到故障代码？ +- 查看 Byte0（行走故障）和 Byte1（转向故障） +- 参考 Curtis 故障代码手册诊断 +- 检查电机连接和电源 +- 检查控制器参数配置 + +### 问：急停无法解除？ +- 检查硬件急停开关是否复位 +- 确认 Byte7 Bit1 状态 +- 发送命令清除软件急停（Byte1 Bit1 = 0） +- 检查安全回路 + +--- + +## 协议版本历史 + +**V4.0** (2021-06-05) +- 当前版本 +- 拟制：zuofuxing + +--- + +## 相关文档 + +- **can-protocol.md** - USB-CAN 接口函数库参考（ControlCAN.dll） +- **CAN_Protocol.pdf** - CAN 通讯协议基础 +- **09-01-14 CAN Protocol Specification** - REMA 手柄通讯协议 + +**技术支持：** 上海诺力智能科技有限公司 + +--- + +## 快速参考 + +### CAN 配置 +``` +波特率: 125 kbps +Timing0: 0x03 +Timing1: 0x1C +``` + +### CAN ID 映射 +``` +AGV → Curtis: + 0x1E5 - 控制命令（行走、提升） + 0x2E5 - 转向角度 + +Curtis → AGV: + 0x260 - 状态反馈 + 0x360 - 保留 +``` + +### 数据范围 +``` +行走速度: -4096 ~ +4095 +提升速度: -4096 ~ +4095 +转向角度命令: -900 ~ +900 (-90° ~ +90°) +转向角度反馈: -16383 ~ +16383 (-90° ~ +90°) +电量: 0 ~ 100 (%) +``` + +### 控制位快速索引 +``` +Byte0 Bit0: 调速开关 (0=有效) +Byte1 Bit0: AGV标识 (1=连接) ← 必须设置 +Byte1 Bit1: 紧急停车 (1=急停) +Byte7 Bit0: 手自动 (1=自动) +Byte7 Bit1: 急停状态 (1=急停) +``` + +### 关键API +```c +Curtis_Initialize() // 初始化控制器 +Curtis_Connect() // 连接（打开设备、初始化CAN、启动） +Curtis_SendCommand() // 发送控制命令（≤20ms周期） +Curtis_ReceiveStatus() // 接收状态反馈 +Curtis_EmergencyStop() // 紧急停车 +Curtis_Disconnect() // 断开连接（发送停止、关闭设备） +``` diff --git a/docs/curtis/CURTIS_INTEGRATION_GUIDE.md b/docs/curtis/CURTIS_INTEGRATION_GUIDE.md new file mode 100644 index 0000000..319f069 --- /dev/null +++ b/docs/curtis/CURTIS_INTEGRATION_GUIDE.md @@ -0,0 +1,751 @@ +# Curtis 电机控制器集成指南 + +## 概述 + +本指南介绍如何在 AGV 路径跟踪项目中集成柯蒂斯（Curtis）1298+1220C 电机控制器的 CAN 通讯功能。 + +**协议版本：** V4.0 +**波特率：** 125 kbps +**厂商：** 上海诺力智能科技有限公司 + +--- + +## 目录 + +1. [快速开始](#快速开始) +2. [文件结构](#文件结构) +3. [编译和运行](#编译和运行) +4. [API 参考](#api-参考) +5. [完整调用流程](#完整调用流程) +6. [示例程序说明](#示例程序说明) +7. [故障排查](#故障排查) +8. [最佳实践](#最佳实践) + +--- + +## 快速开始 + +### 1. 最简示例（5行代码） + +```cpp +#include "include/can/CurtisMotorController.h" + +CurtisMotorController curtis; +curtis.Connect(); // 连接 + +CurtisCommand cmd; +cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(30.0f); // 前进30% +curtis.SendCommand(cmd); // 发送命令 + +curtis.ReceiveStatus(); // 接收状态 +curtis.PrintStatus(); // 打印状态 + +curtis.Disconnect(); // 断开 +``` + +### 2. 基本控制循环 + +```cpp +CurtisMotorController curtis; +curtis.Connect(); + +CurtisCommand cmd; +cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(30.0f); +cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(15.0f); + +while (running) { + curtis.ReceiveStatus(); // 接收状态 + curtis.SendCommand(cmd); // 发送命令（≤20ms周期） + Sleep(15); // 15ms +} + +curtis.Disconnect(); +``` + +--- + +## 文件结构 + +### 核心文件 + +``` +agv_path_tracking/ +├── include/can/ +│ └── CurtisMotorController.h # Curtis 控制器头文件 +├── src/can/ +│ └── CurtisMotorController.cpp # Curtis 控制器实现 +├── examples/ +│ ├── curtis_demo.cpp # 键盘控制示例 +│ └── curtis_path_tracking_demo.cpp # 路径跟踪示例 +├── lib/ +│ ├── ControlCAN.h # CAN 接口库头文件 +│ └── ControlCAN.dll # CAN 接口库 +└── docs/curtis/ + └── CURTIS_INTEGRATION_GUIDE.md # 本文档 +``` + +### 技能文档 + +``` +.claude/skills/ +├── can-protocol.md # USB-CAN 接口函数库参考 +└── curtis-motor-protocol.md # Curtis 协议详细说明 +``` + +--- + +## 编译和运行 + +### 1. 编译 + +```bash +cd build +cmake .. +make +``` + +**编译目标：** +- `curtis_demo` - 键盘控制演示 +- `curtis_path_tracking_demo` - 路径跟踪演示 + +### 2. 运行示例 + +**键盘控制演示：** +```bash +./curtis_demo +``` + +**路径跟踪演示：** +```bash +./curtis_path_tracking_demo +``` + +### 3. 在您的项目中使用 + +**在 CMakeLists.txt 中添加：** +```cmake +add_executable(your_program your_main.cpp + src/can/CurtisMotorController.cpp +) + +target_link_libraries(your_program + ${CMAKE_SOURCE_DIR}/lib/ControlCAN.dll +) +``` + +**在代码中包含头文件：** +```cpp +#include "include/can/CurtisMotorController.h" +``` + +--- + +## API 参考 + +### 核心类：CurtisMotorController + +#### 构造函数 + +```cpp +CurtisMotorController(DWORD deviceType = VCI_USBCAN2, + DWORD deviceIndex = 0, + DWORD canChannel = 0); +``` + +**参数：** +- `deviceType`: CAN 设备类型（默认 VCI_USBCAN2 = 4） +- `deviceIndex`: 设备索引（默认 0 = 第一个设备） +- `canChannel`: CAN 通道（默认 0 = CAN1） + +#### 连接管理 + +```cpp +bool Connect(); // 连接到 Curtis 控制器 +void Disconnect(); // 断开连接 +bool IsConnected() const; // 检查是否已连接 +``` + +#### 命令发送 + +```cpp +bool SendCommand(const CurtisCommand& cmd); // 发送控制命令 +bool SendStopCommand(); // 发送停止命令 +void EmergencyStop(); // 紧急停车 +``` + +**重要：** 必须以 ≤20ms 周期持续发送命令，否则控制器会超时保护。 + +#### 状态接收 + +```cpp +bool ReceiveStatus(); // 接收状态反馈 +const CurtisStatus& GetStatus() const; // 获取当前状态 +void PrintStatus() const; // 打印状态到控制台 +``` + +#### 故障检测 + +```cpp +bool HasError() const; // 检查是否有故障 +bool IsTimeout(uint32_t timeoutMs) const; // 检查通讯超时 +std::string GetErrorString() const; // 获取故障描述 +``` + +#### 工具函数（静态） + +```cpp +static int16_t SpeedPercentToCAN(float percent); // 速度百分比 → CAN值 +static float SpeedCANToPercent(int16_t canValue); // CAN值 → 速度百分比 +static int16_t AngleDegreeToCAN(float degrees); // 角度 → CAN值 +static float AngleCANToDegree(int16_t canValue); // CAN值 → 角度 +``` + +### 数据结构 + +#### CurtisCommand（控制命令） + +```cpp +struct CurtisCommand { + int16_t driveSpeed; // 行走速度 -4096~4095 + int16_t liftSpeedLeft; // 左侧提升速度 -4096~4095 + int16_t liftSpeedRight; // 右侧提升速度 -4096~4095 + int16_t steerAngle; // 转向角度 -900~900 + uint8_t controlBits0; // 控制位0 + uint8_t controlBits1; // 控制位1（自动设置AGV标识） +}; +``` + +#### CurtisStatus（状态反馈） + +```cpp +struct CurtisStatus { + uint8_t driveError; // 行走故障代码 + uint8_t steerError; // 转向故障代码 + float currentAngle; // 当前转向角度（度） + int16_t motorSpeed; // 电机转速 + uint8_t batteryLevel; // 电量 0-100% + bool isAutoMode; // 是否自动模式 + bool isEmergencyStop; // 是否急停状态 + uint32_t lastUpdateTime; // 最后更新时间戳 +}; +``` + +--- + +## 完整调用流程 + +### 流程图 + +``` +┌─────────────────────────────────────────────┐ +│ 1. 创建 CurtisMotorController 对象 │ +└─────────────────┬───────────────────────────┘ + │ + ▼ +┌─────────────────────────────────────────────┐ +│ 2. Connect() │ +│ ├─ VCI_OpenDevice() 打开CAN设备 │ +│ ├─ VCI_InitCAN() 初始化(125kbps) │ +│ ├─ VCI_StartCAN() 启动CAN通道 │ +│ └─ VCI_ClearBuffer() 清空缓冲区 │ +└─────────────────┬───────────────────────────┘ + │ + ▼ +┌─────────────────────────────────────────────┐ +│ 3. 主循环（≤15ms 周期） │ +│ ┌──────────────────────────────────────┐ │ +│ │ ① ReceiveStatus() │ │ +│ │ └─ 接收并解析 0x260 状态帧 │ │ +│ │ │ │ +│ │ ② 检查故障和急停 │ │ +│ │ ├─ HasError() │ │ +│ │ ├─ IsTimeout() │ │ +│ │ └─ 如有异常，执行 EmergencyStop() │ │ +│ │ │ │ +│ │ ③ 设置控制命令 │ │ +│ │ ├─ SpeedPercentToCAN() │ │ +│ │ └─ AngleDegreeToCAN() │ │ +│ │ │ │ +│ │ ④ SendCommand() │ │ +│ │ ├─ 发送 0x1E5 行走/提升命令 │ │ +│ │ └─ 发送 0x2E5 转向命令 │ │ +│ │ │ │ +│ │ ⑤ Sleep(15) │ │ +│ └──────────────────────────────────────┘ │ +└─────────────────┬───────────────────────────┘ + │ + ▼ +┌─────────────────────────────────────────────┐ +│ 4. Disconnect() │ +│ ├─ SendStopCommand() 发送停止命令 │ +│ └─ VCI_CloseDevice() 关闭设备 │ +└─────────────────────────────────────────────┘ +``` + +### 详细步骤说明 + +#### 步骤1：创建对象 + +```cpp +// 使用默认参数（设备类型=VCI_USBCAN2, 索引=0, 通道=0） +CurtisMotorController curtis; + +// 或指定参数 +CurtisMotorController curtis(VCI_USBCAN2, 0, 0); +``` + +#### 步骤2：连接 + +```cpp +if (!curtis.Connect()) { + std::cerr << "连接失败！" << std::endl; + return -1; +} +``` + +**内部执行：** +1. `VCI_OpenDevice()` - 打开 USB-CAN 设备 +2. `VCI_InitCAN()` - 配置 125kbps 波特率 +3. `VCI_StartCAN()` - 启动 CAN 通道 +4. `VCI_ClearBuffer()` - 清空接收缓冲区 + +#### 步骤3：主循环 + +```cpp +CurtisCommand cmd; +DWORD lastTime = GetTickCount(); + +while (running) { + // 3.1 接收状态 + curtis.ReceiveStatus(); + const CurtisStatus& status = curtis.GetStatus(); + + // 3.2 检查故障 + if (curtis.HasError()) { + std::cout << curtis.GetErrorString() << std::endl; + } + + // 3.3 检查急停 + if (status.isEmergencyStop) { + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + } + + // 3.4 设置命令 + cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(30.0f); + cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(15.0f); + + // 3.5 发送命令（15ms周期） + if (GetTickCount() - lastTime >= 15) { + curtis.SendCommand(cmd); + lastTime = GetTickCount(); + } + + Sleep(5); +} +``` + +**关键点：** +- **命令周期：** 必须 ≤20ms，推荐 10-15ms +- **状态接收：** 每次循环都调用 `ReceiveStatus()` +- **故障检测：** 实时检查故障和急停状态 +- **数值转换：** 使用工具函数进行单位转换 + +#### 步骤4：断开连接 + +```cpp +curtis.Disconnect(); +``` + +**内部执行：** +1. 发送停止命令（所有速度归零） +2. 等待 50ms 确保命令发送 +3. 调用 `VCI_CloseDevice()` 关闭设备 + +--- + +## 示例程序说明 + +### 示例1：键盘控制（curtis_demo.cpp） + +**功能：** +- 键盘控制 AGV 运动 +- 实时状态监控 +- 故障和急停检测 + +**控制键：** +- `W/S` - 前进/后退（30%） +- `A/D` - 左转/右转（30度） +- `Q/E` - 提升/下降（50%） +- `H` - 鸣喇叭 +- `T` - 切换龟速模式 +- `空格` - 全部停止 +- `P` - 打印状态 +- `ESC` - 退出 + +**运行：** +```bash +cd build +./curtis_demo +``` + +**核心代码：** +```cpp +// 主循环 +while (running) { + curtis.ReceiveStatus(); // 接收状态 + + if (_kbhit()) { + char ch = _getch(); + switch (ch) { + case 'w': + cmd.driveSpeed = SpeedPercentToCAN(30.0f); + break; + // ... 其他键处理 + } + } + + if (GetTickCount() - lastSendTime >= 15) { + curtis.SendCommand(cmd); // 15ms周期 + lastSendTime = GetTickCount(); + } + + Sleep(5); +} +``` + +### 示例2：路径跟踪（curtis_path_tracking_demo.cpp） + +**功能：** +- 矩形路径跟踪 +- 圆形路径跟踪 +- 实时位置控制 +- 故障自动中止 + +**运行：** +```bash +cd build +./curtis_path_tracking_demo +``` + +**核心代码：** +```cpp +for (size_t i = 0; i < path.size(); i++) { + double targetX = path[i].x; + double targetY = path[i].y; + + while (true) { + curtis.ReceiveStatus(); + + // 计算距离和方向 + double dx = targetX - currentX; + double dy = targetY - currentY; + double distance = sqrt(dx*dx + dy*dy); + + if (distance < 0.1) break; // 到达 + + // 计算转向角度和速度 + double targetAngle = atan2(dy, dx); + cmd.steerAngle = AngleDegreeToCAN(angleError); + cmd.driveSpeed = SpeedPercentToCAN(30.0f); + + curtis.SendCommand(cmd); + Sleep(15); + } +} +``` + +--- + +## 故障排查 + +### 常见问题 + +#### 1. 连接失败 + +**症状：** `Connect()` 返回 false + +**解决方案：** +- 检查 USB-CAN 设备是否连接 +- 检查驱动是否正确安装 +- 确认 `ControlCAN.dll` 在正确位置 +- 检查设备索引是否正确（通常为 0） + +**诊断命令：** +```cpp +VCI_BOARD_INFO info; +if (VCI_ReadBoardInfo(VCI_USBCAN2, 0, &info) == 1) { + cout << "设备序列号: " << info.str_Serial_Num << endl; +} +``` + +#### 2. 控制器无响应 + +**症状：** 发送命令后车辆不动 + +**检查清单：** +- [ ] AGV 标识位是否设置（Byte1 Bit0 = 1） +- [ ] 命令发送频率是否 ≤20ms +- [ ] 手动模式下 J9/SW_3 是否闭合 +- [ ] 自动模式下 J24/SW_1 是否为 ON +- [ ] 波特率是否为 125kbps + +**诊断代码：** +```cpp +const CurtisCommand& lastCmd = curtis.GetLastCommand(); +cout << "最后命令: " << endl; +cout << " driveSpeed = " << lastCmd.driveSpeed << endl; +cout << " steerAngle = " << lastCmd.steerAngle << endl; +cout << " controlBits1 = 0x" << hex << (int)lastCmd.controlBits1 << endl; +``` + +#### 3. 频繁超时 + +**症状：** `IsTimeout()` 频繁返回 true + +**原因：** +- 命令发送周期 > 20ms +- CAN 总线连接不稳定 +- 控制器参数设置不正确 + +**解决方案：** +```cpp +// 检查发送周期 +DWORD lastTime = GetTickCount(); +curtis.SendCommand(cmd); +DWORD elapsed = GetTickCount() - lastTime; +cout << "发送耗时: " << elapsed << "ms" << endl; // 应 < 20ms +``` + +#### 4. 转向不准确 + +**症状：** 转向角度与预期不符 + +**解决方案：** +- 确认 1220C 参数配置： + ``` + can steer left stop to centre = -900 + can steer right stop to centre = 900 + ``` +- 检查角度转换： + ```cpp + float degrees = 30.0f; + int16_t canValue = CurtisMotorController::AngleDegreeToCAN(degrees); + cout << "30° → CAN值 = " << canValue << endl; // 应为 300 + ``` + +#### 5. 收到故障代码 + +**症状：** `HasError()` 返回 true + +**诊断：** +```cpp +const CurtisStatus& status = curtis.GetStatus(); +cout << "行走故障: 0x" << hex << (int)status.driveError << endl; +cout << "转向故障: 0x" << hex << (int)status.steerError << endl; +cout << curtis.GetErrorString() << endl; +``` + +**处理：** +- 查阅 Curtis 故障代码手册 +- 检查电机连接和电源 +- 检查控制器参数配置 + +--- + +## 最佳实践 + +### 1. 命令发送频率 + +**推荐做法：** +```cpp +const int SEND_PERIOD_MS = 15; // 15ms周期 +DWORD lastSendTime = GetTickCount(); + +while (running) { + if (GetTickCount() - lastSendTime >= SEND_PERIOD_MS) { + curtis.SendCommand(cmd); + lastSendTime = GetTickCount(); + } + Sleep(5); +} +``` + +**避免：** +- ❌ 周期 > 20ms（会超时） +- ❌ 不规律的发送间隔 +- ❌ 只发送一次就停止 + +### 2. 故障处理 + +**推荐做法：** +```cpp +curtis.ReceiveStatus(); +const CurtisStatus& status = curtis.GetStatus(); + +// 实时检查故障 +if (curtis.HasError()) { + cout << "[故障] " << curtis.GetErrorString() << endl; + curtis.EmergencyStop(); + // 记录日志 + logError(status.driveError, status.steerError); + // 通知上层 + notifyError(); +} + +// 检查急停 +if (status.isEmergencyStop) { + cout << "[急停] 停止所有运动" << endl; + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + curtis.SendCommand(cmd); +} +``` + +### 3. 数值范围检查 + +**推荐做法：** +```cpp +// 使用工具函数自动限制范围 +float speedPercent = 120.0f; // 超出范围 +int16_t canSpeed = CurtisMotorController::SpeedPercentToCAN(speedPercent); +// 自动限制到 100% → 4095 + +// 或手动检查 +cmd.driveSpeed = canSpeed; +if (cmd.driveSpeed > CURTIS_DRIVE_SPEED_MAX) { + cmd.driveSpeed = CURTIS_DRIVE_SPEED_MAX; +} +``` + +### 4. 状态回调 + +**推荐做法：** +```cpp +curtis.SetStatusCallback([](const CurtisStatus& status) { + // 状态更新时自动调用 + if (status.batteryLevel < 20) { + cout << "[警告] 电量低: " << (int)status.batteryLevel << "%" << endl; + } +}); +``` + +### 5. 安全断开 + +**推荐做法：** +```cpp +// 程序退出前 +cout << "正在安全停止..." << endl; +curtis.SendStopCommand(); +Sleep(100); // 等待停止生效 +curtis.Disconnect(); +``` + +### 6. 异常处理 + +**推荐做法：** +```cpp +try { + CurtisMotorController curtis; + if (!curtis.Connect()) { + throw runtime_error("连接失败"); + } + + // 主循环... + +} catch (const exception& e) { + cerr << "异常: " << e.what() << endl; + curtis.Disconnect(); + return -1; +} +``` + +### 7. 日志记录 + +**推荐做法：** +```cpp +void logStatus(const CurtisStatus& status) { + ofstream log("curtis_log.txt", ios::app); + log << GetTickCount() << "," + << (int)status.driveError << "," + << (int)status.steerError << "," + << status.currentAngle << "," + << status.motorSpeed << "," + << (int)status.batteryLevel << endl; +} + +// 定期调用 +if (GetTickCount() - lastLogTime >= 1000) { + logStatus(curtis.GetStatus()); + lastLogTime = GetTickCount(); +} +``` + +--- + +## 附录 + +### A. CAN ID 映射表 + +| CAN ID | 方向 | 描述 | 数据内容 | +|--------|------|------|----------| +| 0x1E5 | AGV → Curtis | 控制命令 | 行走速度、提升速度、控制位 | +| 0x2E5 | AGV → Curtis | 转向命令 | 转向角度 | +| 0x260 | Curtis → AGV | 状态反馈 | 故障、角度、转速、电量、状态 | +| 0x360 | Curtis → AGV | 保留 | 保留字段 | + +### B. 控制位定义 + +**控制位0（Byte0）：** +| Bit | 功能 | 0 | 1 | +|-----|------|---|---| +| 0 | 调速开关 | 有效 | 无效 | +| 1 | 紧急反向 | 无效 | 有效 | +| 2 | 龟速模式 | 无效 | 有效 | +| 3 | 喇叭 | 无效 | 有效 | +| 4 | 左提升 | 无效 | 有效 | +| 5 | 左下降 | 无效 | 有效 | +| 6 | 右提升 | 无效 | 有效 | +| 7 | 右下降 | 无效 | 有效 | + +**控制位1（Byte1 - AGV专用）：** +| Bit | 功能 | 0 | 1 | +|-----|------|---|---| +| 0 | AGV标识 | 无效 | 连接（必须） | +| 1 | 紧急停车 | 无效 | 急停 | +| 2 | 找中完成 | - | 完成 | +| 3-7 | 保留 | - | - | + +### C. 数据范围表 + +| 参数 | 最小值 | 最大值 | 单位 | 说明 | +|------|--------|--------|------|------| +| 行走速度 | -4096 | 4095 | - | 负值=后退 | +| 提升速度 | -4096 | 4095 | - | 负值=下降 | +| 转向角度（命令） | -900 | 900 | 0.1° | -900=-90° | +| 转向角度（反馈） | -16383 | 16383 | 0.1° | -16383=-90° | +| 电量 | 0 | 100 | % | 百分比 | + +### D. 相关文档 + +- **can-protocol.md** - USB-CAN 接口函数库参考 +- **curtis-motor-protocol.md** - Curtis 协议详细说明 +- **CAN_Protocol.pdf** - CAN 通讯协议基础 +- **柯蒂斯通信协议.pdf** - 原始协议文档 + +--- + +## 技术支持 + +如有问题，请参考： +1. `.claude/skills/curtis-motor-protocol.md` - 完整协议说明 +2. `.claude/skills/can-protocol.md` - CAN 接口库文档 +3. 示例程序源码 + +**厂商技术支持：** 上海诺力智能科技有限公司 + +--- + +**文档版本：** 1.0 +**最后更新：** 2024-11-15 +**作者：** AGV 路径跟踪项目组 diff --git a/docs/curtis/QUICKSTART.md b/docs/curtis/QUICKSTART.md new file mode 100644 index 0000000..a252ca4 --- /dev/null +++ b/docs/curtis/QUICKSTART.md @@ -0,0 +1,79 @@ +# Curtis 电机控制器快速使用指南 + +## 已创建的文件 + +### 核心代码 +- `include/can/CurtisMotorController.h` - Curtis 控制器头文件 +- `src/can/CurtisMotorController.cpp` - Curtis 控制器实现 + +### 示例程序 +- `examples/curtis_demo.cpp` - 键盘控制演示 +- `examples/curtis_path_tracking_demo.cpp` - 路径跟踪演示 + +### 文档 +- `docs/curtis/CURTIS_INTEGRATION_GUIDE.md` - 完整集成指南 + +## 快速编译和运行 + +### 1. 编译 + +```bash +cd build +cmake .. +make +``` + +### 2. 运行示例 + +**键盘控制演示：** +```bash +./curtis_demo +``` + +**路径跟踪演示：** +```bash +./curtis_path_tracking_demo +``` + +## 完整调用流程（简化版） + +```cpp +#include "include/can/CurtisMotorController.h" + +int main() { + // 1. 创建对象 + CurtisMotorController curtis; + + // 2. 连接 + curtis.Connect(); + + // 3. 主循环（≤15ms周期） + CurtisCommand cmd; + while (running) { + // 接收状态 + curtis.ReceiveStatus(); + + // 设置命令 + cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(30.0f); + cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(15.0f); + + // 发送命令 + curtis.SendCommand(cmd); + + Sleep(15); + } + + // 4. 断开 + curtis.Disconnect(); + return 0; +} +``` + +## 详细文档 + +请参阅：`docs/curtis/CURTIS_INTEGRATION_GUIDE.md` + +## 技能文档 + +- `.claude/skills/curtis-motor-protocol.md` - Curtis 协议详细说明 +- `.claude/skills/can-protocol.md` - CAN 接口库文档 diff --git a/examples/curtis_demo.cpp b/examples/curtis_demo.cpp new file mode 100644 index 0000000..171ac16 --- /dev/null +++ b/examples/curtis_demo.cpp @@ -0,0 +1,232 @@ +/** + * @file curtis_demo.cpp + * @brief Curtis 电机控制器键盘控制演示程序 + * + * 功能说明： + * - 演示 Curtis 电机控制器的完整使用流程 + * - 提供键盘控制接口（WASD + QE + 空格 + ESC） + * - 实时显示控制器状态 + * - 故障和急停检测 + * + * 编译： + * cd build + * cmake .. + * make curtis_demo + * + * 运行： + * ./curtis_demo + */ + +#include "../include/can/CurtisMotorController.h" +#include +#include +#include + +using namespace std; + +/** + * @brief 打印控制说明 + */ +void printInstructions() { + cout << "\n===== Curtis 电机控制器键盘控制演示 =====" << endl; + cout << "\n控制说明：" << endl; + cout << " W/S - 前进/后退 (30%速度)" << endl; + cout << " A/D - 左转/右转 (30度)" << endl; + cout << " Q/E - 提升/下降 (50%速度)" << endl; + cout << " H - 鸣喇叭" << endl; + cout << " T - 切换龟速模式" << endl; + cout << " 空格 - 全部停止" << endl; + cout << " ESC - 退出程序" << endl; + cout << " P - 打印状态" << endl; + cout << "==========================================\n" << endl; +} + +/** + * @brief 主函数 + */ +int main() { + cout << "===== Curtis 电机控制器 CAN 通讯演示 =====\n" << endl; + + // ============ 1. 创建控制器对象 ============ + cout << "步骤 1: 创建 Curtis 控制器对象..." << endl; + CurtisMotorController curtis(VCI_USBCAN2, 0, 0); + + // ============ 2. 连接 ============ + cout << "\n步骤 2: 连接到 Curtis 控制器..." << endl; + if (!curtis.Connect()) { + cerr << "连接失败！请检查：" << endl; + cerr << " 1. CAN 设备是否已连接" << endl; + cerr << " 2. 驱动是否正确安装" << endl; + cerr << " 3. 设备权限是否正确" << endl; + return -1; + } + + // 等待控制器稳定 + Sleep(100); + + // ============ 3. 主控制循环 ============ + cout << "\n步骤 3: 进入主控制循环" << endl; + printInstructions(); + + bool running = true; + bool turtleMode = false; // 龟速模式 + + CurtisCommand cmd; + cmd.controlBits0 = 0x00; // 调速开关有效（bit0=0） + cmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED; + + DWORD lastSendTime = GetTickCount(); + DWORD lastPrintTime = GetTickCount(); + DWORD lastStatusCheckTime = GetTickCount(); + int loopCount = 0; + int errorCount = 0; + + // 设置状态回调（可选） + curtis.SetStatusCallback([](const CurtisStatus& status) { + // 状态更新时自动调用 + if (status.driveError || status.steerError) { + cout << "[警告] 检测到故障！" << endl; + } + }); + + while (running) { + DWORD currentTime = GetTickCount(); + + // ---- 接收状态（每次循环） ---- + curtis.ReceiveStatus(); + + // ---- 键盘控制 ---- + if (_kbhit()) { + char ch = _getch(); + + switch (ch) { + case 'w': case 'W': + cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(30.0f); + cout << "[控制] 前进 30%" << endl; + break; + + case 's': case 'S': + cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(-30.0f); + cout << "[控制] 后退 30%" << endl; + break; + + case 'a': case 'A': + cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(-30.0f); + cout << "[控制] 左转 30°" << endl; + break; + + case 'd': case 'D': + cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(30.0f); + cout << "[控制] 右转 30°" << endl; + break; + + case 'q': case 'Q': + cmd.liftSpeedLeft = CurtisMotorController::SpeedPercentToCAN(50.0f); + cout << "[控制] 提升" << endl; + break; + + case 'e': case 'E': + cmd.liftSpeedLeft = CurtisMotorController::SpeedPercentToCAN(-50.0f); + cout << "[控制] 下降" << endl; + break; + + case 'h': case 'H': + cmd.controlBits0 |= CURTIS_CTRL_HORN; + cout << "[控制] 鸣喇叭" << endl; + Sleep(500); + cmd.controlBits0 &= ~CURTIS_CTRL_HORN; + break; + + case 't': case 'T': + turtleMode = !turtleMode; + if (turtleMode) { + cmd.controlBits0 |= CURTIS_CTRL_TURTLE_MODE; + cout << "[控制] 龟速模式 ON" << endl; + } else { + cmd.controlBits0 &= ~CURTIS_CTRL_TURTLE_MODE; + cout << "[控制] 龟速模式 OFF" << endl; + } + break; + + case ' ': // 空格 - 全部停止 + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + cmd.liftSpeedLeft = 0; + cmd.liftSpeedRight = 0; + cmd.controlBits0 &= ~(CURTIS_CTRL_HORN | CURTIS_CTRL_TURTLE_MODE); + cout << "[控制] 全部停止" << endl; + break; + + case 'p': case 'P': + curtis.PrintStatus(); + break; + + case 27: // ESC - 退出 + cout << "\n[控制] 用户请求退出" << endl; + running = false; + break; + + default: + break; + } + } + + // ---- 定期检查故障和状态（每500ms） ---- + if (currentTime - lastStatusCheckTime >= 500) { + const CurtisStatus& status = curtis.GetStatus(); + + // 检查故障 + if (status.driveError != 0 || status.steerError != 0) { + cout << "[警告] " << curtis.GetErrorString() << endl; + errorCount++; + } + + // 检查急停 + if (status.isEmergencyStop) { + cout << "[警告] 急停状态！停止所有运动" << endl; + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + cmd.liftSpeedLeft = 0; + cmd.liftSpeedRight = 0; + } + + // 检查通讯超时 + if (curtis.IsTimeout(200)) { + cout << "[警告] 通讯超时！" << endl; + } + + lastStatusCheckTime = currentTime; + } + + // ---- 定时发送命令（15ms周期，≤20ms） ---- + if (currentTime - lastSendTime >= 15) { + if (!curtis.SendCommand(cmd)) { + cerr << "[错误] 发送命令失败！" << endl; + running = false; + } + lastSendTime = currentTime; + } + + // ---- 定时打印状态（5秒） ---- + if (currentTime - lastPrintTime >= 5000) { + curtis.PrintStatus(); + cout << "[信息] 循环次数: " << loopCount + << ", 错误次数: " << errorCount << endl; + lastPrintTime = currentTime; + } + + // 避免CPU占用过高 + Sleep(5); + loopCount++; + } + + cout << "\n[统计] 总循环次数: " << loopCount << endl; + cout << "[统计] 错误次数: " << errorCount << endl; + + // ============ 4. 断开连接 ============ + cout << "\n步骤 4: 断开连接..." << endl; + curtis.Disconnect(); + + cout << "\n===== 程序结束 =====" << endl; + return 0; +} diff --git a/examples/curtis_path_tracking_demo.cpp b/examples/curtis_path_tracking_demo.cpp new file mode 100644 index 0000000..508788d --- /dev/null +++ b/examples/curtis_path_tracking_demo.cpp @@ -0,0 +1,283 @@ +/** + * @file curtis_path_tracking_demo.cpp + * @brief Curtis 电机控制器路径跟踪演示程序 + * + * 功能说明： + * - 演示如何使用 Curtis 控制器进行路径跟踪 + * - 结合 path_tracker 和 CurtisMotorController + * - 实时故障检测和紧急停止 + * - 提供矩形路径和圆形路径示例 + * + * 编译： + * cd build + * cmake .. + * make curtis_path_tracking_demo + * + * 运行： + * ./curtis_path_tracking_demo + */ + +#include "../include/can/CurtisMotorController.h" +#include "../include/path_tracker.h" +#include "../include/path_curve.h" +#include "../include/agv_model.h" +#include +#include +#include +#include + +using namespace std; + +/** + * @brief 路径点结构 + */ +struct PathPoint { + double x; + double y; + double targetSpeed; ///< 目标速度百分比 + + PathPoint(double x_, double y_, double speed_ = 30.0) + : x(x_), y(y_), targetSpeed(speed_) {} +}; + +/** + * @brief 生成矩形路径 + */ +vector generateRectanglePath() { + vector path; + + // 5m x 3m 矩形 + path.push_back(PathPoint(0.0, 0.0, 30.0)); // 起点 + path.push_back(PathPoint(5.0, 0.0, 30.0)); // 直行5米 + path.push_back(PathPoint(5.0, 3.0, 25.0)); // 右转前进3米 + path.push_back(PathPoint(0.0, 3.0, 25.0)); // 左转前进5米 + path.push_back(PathPoint(0.0, 0.0, 20.0)); // 返回起点 + + return path; +} + +/** + * @brief 生成圆形路径 + */ +vector generateCirclePath(double radius = 3.0, int points = 36) { + vector path; + + for (int i = 0; i <= points; i++) { + double angle = 2.0 * M_PI * i / points; + double x = radius * cos(angle); + double y = radius * sin(angle); + path.push_back(PathPoint(x, y, 25.0)); + } + + return path; +} + +/** + * @brief 执行路径跟踪 + */ +bool executePathTracking(CurtisMotorController& curtis, + const vector& path, + AGVModel& agv, + PathTracker& tracker) { + if (path.empty()) { + cerr << "[路径跟踪] 错误: 路径为空" << endl; + return false; + } + + cout << "[路径跟踪] 开始执行，共 " << path.size() << " 个路径点" << endl; + + CurtisCommand cmd; + cmd.controlBits0 = 0x00; + cmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED; + + bool success = true; + + for (size_t i = 0; i < path.size(); i++) { + cout << "\n[路径跟踪] 路径点 " << (i + 1) << "/" << path.size() + << ": (" << path[i].x << ", " << path[i].y + << "), 速度=" << path[i].targetSpeed << "%" << endl; + + // 设置目标点 + double targetX = path[i].x; + double targetY = path[i].y; + + // 持续跟踪该路径点，直到到达 + DWORD startTime = GetTickCount(); + const DWORD maxTime = 10000; // 最大10秒 + + while (GetTickCount() - startTime < maxTime) { + // 接收 Curtis 状态 + curtis.ReceiveStatus(); + const CurtisStatus& status = curtis.GetStatus(); + + // 检查故障 + if (status.driveError || status.steerError) { + cerr << "[路径跟踪] 检测到故障: " << curtis.GetErrorString() << endl; + curtis.EmergencyStop(); + return false; + } + + // 检查急停 + if (status.isEmergencyStop) { + cerr << "[路径跟踪] 检测到急停，中止路径跟踪" << endl; + return false; + } + + // 获取 AGV 当前状态（这里使用模拟数据，实际应用中应从传感器获取） + State currentState; + currentState.x = agv.getState().x; + currentState.y = agv.getState().y; + currentState.theta = agv.getState().theta; + currentState.v = status.motorSpeed / 100.0; // 模拟速度 + currentState.omega = 0.0; + + // 计算到目标点的距离 + double dx = targetX - currentState.x; + double dy = targetY - currentState.y; + double distance = sqrt(dx * dx + dy * dy); + + // 如果接近目标点（< 0.1米），进入下一个点 + if (distance < 0.1) { + cout << "[路径跟踪] 到达路径点 " << (i + 1) << endl; + break; + } + + // 计算目标方向角 + double targetAngle = atan2(dy, dx); + + // 计算转向角（简化版，实际应使用路径跟踪器） + double angleError = targetAngle - currentState.theta; + // 归一化到 [-π, π] + while (angleError > M_PI) angleError -= 2 * M_PI; + while (angleError < -M_PI) angleError += 2 * M_PI; + + // 转向控制（限制在 ±45度） + double steerAngleDeg = angleError * 180.0 / M_PI; + if (steerAngleDeg > 45.0) steerAngleDeg = 45.0; + if (steerAngleDeg < -45.0) steerAngleDeg = -45.0; + + // 速度控制（根据角度误差调整速度） + double speedScale = 1.0 - abs(angleError) / M_PI; + if (speedScale < 0.3) speedScale = 0.3; // 最小30%速度 + + double actualSpeed = path[i].targetSpeed * speedScale; + + // 设置命令 + cmd.driveSpeed = CurtisMotorController::SpeedPercentToCAN(actualSpeed); + cmd.steerAngle = CurtisMotorController::AngleDegreeToCAN(steerAngleDeg); + + // 发送命令 + if (!curtis.SendCommand(cmd)) { + cerr << "[路径跟踪] 发送命令失败" << endl; + return false; + } + + // 更新 AGV 模型（模拟） + ControlInput control; + control.v = actualSpeed / 100.0; + control.delta = steerAngleDeg * M_PI / 180.0; + agv.update(control, 0.015); // 15ms + + // 打印进度 + if ((GetTickCount() - startTime) % 500 == 0) { + cout << " 位置: (" << currentState.x << ", " << currentState.y + << "), 距离: " << distance << "m" << endl; + } + + Sleep(15); // 15ms周期 + } + + // 超时检查 + if (GetTickCount() - startTime >= maxTime) { + cerr << "[路径跟踪] 超时: 无法到达路径点 " << (i + 1) << endl; + success = false; + break; + } + } + + // 路径结束，停止车辆 + cout << "\n[路径跟踪] 路径执行完成，停止车辆" << endl; + cmd.driveSpeed = 0; + cmd.steerAngle = 0; + for (int i = 0; i < 10; i++) { + curtis.SendCommand(cmd); + Sleep(15); + } + + return success; +} + +/** + * @brief 主函数 + */ +int main() { + cout << "===== Curtis 路径跟踪演示 =====\n" << endl; + + // ============ 1. 创建对象 ============ + cout << "步骤 1: 创建对象..." << endl; + CurtisMotorController curtis(VCI_USBCAN2, 0, 0); + + // 创建 AGV 模型（用于模拟） + AGVModel agv; + State initialState; + initialState.x = 0.0; + initialState.y = 0.0; + initialState.theta = 0.0; + initialState.v = 0.0; + initialState.omega = 0.0; + agv.setState(initialState); + + // 创建路径跟踪器 + PathTracker tracker; + + // ============ 2. 连接 ============ + cout << "\n步骤 2: 连接到 Curtis 控制器..." << endl; + if (!curtis.Connect()) { + cerr << "连接失败！" << endl; + return -1; + } + + Sleep(100); + + // ============ 3. 选择路径 ============ + cout << "\n步骤 3: 选择路径类型" << endl; + cout << "1 - 矩形路径 (5m x 3m)" << endl; + cout << "2 - 圆形路径 (半径 3m)" << endl; + cout << "请选择 (1/2): "; + + int choice; + cin >> choice; + + vector path; + if (choice == 1) { + path = generateRectanglePath(); + cout << "已选择: 矩形路径" << endl; + } else if (choice == 2) { + path = generateCirclePath(); + cout << "已选择: 圆形路径" << endl; + } else { + cerr << "无效选择，使用默认矩形路径" << endl; + path = generateRectanglePath(); + } + + // ============ 4. 执行路径跟踪 ============ + cout << "\n步骤 4: 执行路径跟踪" << endl; + cout << "按 Enter 开始..." << endl; + cin.ignore(); + cin.get(); + + bool success = executePathTracking(curtis, path, agv, tracker); + + if (success) { + cout << "\n✓ 路径跟踪成功完成" << endl; + } else { + cout << "\n✗ 路径跟踪失败" << endl; + } + + // ============ 5. 断开连接 ============ + cout << "\n步骤 5: 断开连接..." << endl; + curtis.Disconnect(); + + cout << "\n===== 程序结束 =====" << endl; + return 0; +} diff --git a/include/can/CurtisMotorController.h b/include/can/CurtisMotorController.h new file mode 100644 index 0000000..65af9e8 --- /dev/null +++ b/include/can/CurtisMotorController.h @@ -0,0 +1,297 @@ +/** + * @file CurtisMotorController.h + * @brief 柯蒂斯（Curtis）1298+1220C 电机控制器 CAN 通讯接口 + * @details 封装柯蒂斯电机控制器的 CAN 通讯协议，提供易用的控制接口 + * + * 协议版本: V4.0 + * 波特率: 125 kbps + * CAN ID: + * - 0x1E5: AGV → Curtis 控制命令（行走、提升） + * - 0x2E5: AGV → Curtis 转向命令 + * - 0x260: Curtis → AGV 状态反馈 + * - 0x360: Curtis → AGV 保留 + */ + +#ifndef CURTIS_MOTOR_CONTROLLER_H +#define CURTIS_MOTOR_CONTROLLER_H + +#include "../../lib/ControlCAN.h" +#include +#include +#include + +// ==================== CAN ID 定义 ==================== +#define CURTIS_ID_DRIVE_CMD 0x1E5 ///< 行走/提升命令 +#define CURTIS_ID_STEER_CMD 0x2E5 ///< 转向命令 +#define CURTIS_ID_STATUS 0x260 ///< 状态反馈 +#define CURTIS_ID_RESERVED 0x360 ///< 保留 + +// ==================== 数据范围定义 ==================== +#define CURTIS_DRIVE_SPEED_MIN -4096 +#define CURTIS_DRIVE_SPEED_MAX 4095 +#define CURTIS_LIFT_SPEED_MIN -4096 +#define CURTIS_LIFT_SPEED_MAX 4095 +#define CURTIS_STEER_ANGLE_MIN -900 ///< -90度 +#define CURTIS_STEER_ANGLE_MAX 900 ///< +90度 + +// ==================== 控制位掩码定义（Byte0）==================== +#define CURTIS_CTRL_SPEED_SWITCH 0x01 ///< 调速开关（0=有效） +#define CURTIS_CTRL_EMERGENCY_REVERSE 0x02 ///< 紧急反向 +#define CURTIS_CTRL_TURTLE_MODE 0x04 ///< 龟速开关 +#define CURTIS_CTRL_HORN 0x08 ///< 喇叭 +#define CURTIS_CTRL_LIFT_LEFT_UP 0x10 ///< 左侧提升 +#define CURTIS_CTRL_LIFT_LEFT_DOWN 0x20 ///< 左侧下降 +#define CURTIS_CTRL_LIFT_RIGHT_UP 0x40 ///< 右侧提升 +#define CURTIS_CTRL_LIFT_RIGHT_DOWN 0x80 ///< 右侧下降 + +// ==================== 控制位掩码定义（Byte1 - AGV专用）==================== +#define CURTIS_CTRL_AGV_CONNECTED 0x01 ///< AGV连接标识（必须设置） +#define CURTIS_CTRL_EMERGENCY_STOP 0x02 ///< 紧急停车 +#define CURTIS_CTRL_CENTERING_DONE 0x04 ///< 找中完成 + +// ==================== 状态位掩码定义（Byte7）==================== +#define CURTIS_STATUS_AUTO_MODE 0x01 ///< 0=手动, 1=自动 +#define CURTIS_STATUS_EMERGENCY_STOP 0x02 ///< 0=无急停, 1=急停 + +/** + * @brief Curtis 控制器状态结构 + */ +struct CurtisStatus { + uint8_t driveError; ///< 行走故障代码 + uint8_t steerError; ///< 转向故障代码 + float currentAngle; ///< 当前转向角度（度） + int16_t motorSpeed; ///< 电机转速 + uint8_t batteryLevel; ///< 电量百分比 0-100 + bool isAutoMode; ///< 是否自动模式 + bool isEmergencyStop; ///< 是否急停状态 + uint32_t lastUpdateTime; ///< 最后更新时间戳（毫秒） + + CurtisStatus() : driveError(0), steerError(0), currentAngle(0.0f), + motorSpeed(0), batteryLevel(0), isAutoMode(false), + isEmergencyStop(false), lastUpdateTime(0) {} +}; + +/** + * @brief Curtis 控制命令结构 + */ +struct CurtisCommand { + int16_t driveSpeed; ///< 行走速度 -4096~4095 + int16_t liftSpeedLeft; ///< 左侧提升速度 -4096~4095 + int16_t liftSpeedRight; ///< 右侧提升速度 -4096~4095 + int16_t steerAngle; ///< 转向角度 -900~900 + uint8_t controlBits0; ///< 控制位0 + uint8_t controlBits1; ///< 控制位1（AGV专用） + + CurtisCommand() : driveSpeed(0), liftSpeedLeft(0), liftSpeedRight(0), + steerAngle(0), controlBits0(0), controlBits1(CURTIS_CTRL_AGV_CONNECTED) {} +}; + +/** + * @brief 柯蒂斯电机控制器类 + * + * 提供完整的柯蒂斯 1298+1220C 电机控制器 CAN 通讯功能 + * + * 使用流程： + * 1. 创建对象 + * 2. 调用 Connect() 连接设备 + * 3. 在主循环中以 ≤20ms 周期调用 SendCommand() 和 ReceiveStatus() + * 4. 结束时调用 Disconnect() + * + * @code + * CurtisMotorController curtis; + * curtis.Connect(); + * + * while (running) { + * CurtisCommand cmd; + * cmd.driveSpeed = SpeedPercentToCAN(30.0f); + * curtis.SendCommand(cmd); + * curtis.ReceiveStatus(); + * Sleep(15); // 15ms周期 + * } + * + * curtis.Disconnect(); + * @endcode + */ +class CurtisMotorController { +public: + /** + * @brief 状态回调函数类型 + * @param status 更新后的状态 + */ + using StatusCallback = std::function; + + /** + * @brief 构造函数 + * @param deviceType CAN 设备类型（VCI_USBCAN2 = 4） + * @param deviceIndex 设备索引（第几个设备，从0开始） + * @param canChannel CAN 通道（0 = CAN1, 1 = CAN2） + */ + CurtisMotorController(DWORD deviceType = VCI_USBCAN2, + DWORD deviceIndex = 0, + DWORD canChannel = 0); + + /** + * @brief 析构函数，自动断开连接 + */ + ~CurtisMotorController(); + + // ==================== 连接管理 ==================== + + /** + * @brief 连接到 Curtis 控制器 + * @details 执行完整的连接流程： + * 1. 打开 CAN 设备 + * 2. 初始化 CAN（125kbps） + * 3. 启动 CAN 通道 + * 4. 清空缓冲区 + * @return true=成功, false=失败 + */ + bool Connect(); + + /** + * @brief 断开与 Curtis 控制器的连接 + * @details 执行安全断开流程： + * 1. 发送停止命令 + * 2. 关闭 CAN 设备 + */ + void Disconnect(); + + /** + * @brief 检查是否已连接 + * @return true=已连接, false=未连接 + */ + bool IsConnected() const { return m_isConnected; } + + // ==================== 命令发送 ==================== + + /** + * @brief 发送控制命令到 Curtis 控制器 + * @details 发送两个 CAN 帧： + * - 0x1E5: 行走和提升命令 + * - 0x2E5: 转向命令 + * @param cmd 控制命令 + * @return true=成功, false=失败 + * @note 必须以 ≤20ms 周期持续发送，否则控制器会超时保护 + */ + bool SendCommand(const CurtisCommand& cmd); + + /** + * @brief 发送停止命令 + * @details 所有速度和角度设为0 + * @return true=成功, false=失败 + */ + bool SendStopCommand(); + + /** + * @brief 紧急停车 + * @details 连续发送3次急停命令确保接收 + */ + void EmergencyStop(); + + // ==================== 状态接收 ==================== + + /** + * @brief 接收 Curtis 控制器状态反馈 + * @details 接收并解析 0x260 状态帧，更新内部状态 + * @return true=接收到新状态, false=没有新数据 + */ + bool ReceiveStatus(); + + /** + * @brief 获取当前状态 + * @return 状态结构的引用 + */ + const CurtisStatus& GetStatus() const { return m_status; } + + /** + * @brief 获取上次发送的命令 + * @return 命令结构的引用 + */ + const CurtisCommand& GetLastCommand() const { return m_lastCommand; } + + /** + * @brief 设置状态更新回调函数 + * @param callback 状态更新时调用的函数 + */ + void SetStatusCallback(StatusCallback callback) { + m_statusCallback = callback; + } + + // ==================== 故障和超时检测 ==================== + + /** + * @brief 检查是否有故障 + * @return true=有故障, false=无故障 + */ + bool HasError() const { + return m_status.driveError != 0 || m_status.steerError != 0; + } + + /** + * @brief 检查通讯是否超时 + * @param timeoutMs 超时时间（毫秒） + * @return true=超时, false=正常 + */ + bool IsTimeout(uint32_t timeoutMs = 100) const; + + /** + * @brief 获取故障信息字符串 + * @return 故障描述 + */ + std::string GetErrorString() const; + + // ==================== 工具函数 ==================== + + /** + * @brief 速度百分比转 CAN 值 + * @param percent 速度百分比 -100% ~ +100% + * @return CAN 值 -4096 ~ +4095 + */ + static int16_t SpeedPercentToCAN(float percent); + + /** + * @brief CAN 值转速度百分比 + * @param canValue CAN 值 -4096 ~ +4095 + * @return 速度百分比 -100% ~ +100% + */ + static float SpeedCANToPercent(int16_t canValue); + + /** + * @brief 角度转 CAN 值（转向命令） + * @param degrees 角度 -90° ~ +90° + * @return CAN 值 -900 ~ +900 + */ + static int16_t AngleDegreeToCAN(float degrees); + + /** + * @brief CAN 值转角度（状态反馈） + * @param canValue CAN 值 -16383 ~ +16383 + * @return 角度 -90° ~ +90° + */ + static float AngleCANToDegree(int16_t canValue); + + /** + * @brief 打印当前状态到控制台 + */ + void PrintStatus() const; + +private: + // 设备参数 + DWORD m_deviceType; + DWORD m_deviceIndex; + DWORD m_canChannel; + bool m_isConnected; + + // 状态和命令 + CurtisStatus m_status; + CurtisCommand m_lastCommand; + + // 回调函数 + StatusCallback m_statusCallback; + + // 内部辅助函数 + bool SendFrame(UINT canId, const BYTE* data, BYTE len); + void ParseStatusFrame(const VCI_CAN_OBJ& frame); +}; + +#endif // CURTIS_MOTOR_CONTROLLER_H diff --git a/src/can/CurtisMotorController.cpp b/src/can/CurtisMotorController.cpp new file mode 100644 index 0000000..76fcef6 --- /dev/null +++ b/src/can/CurtisMotorController.cpp @@ -0,0 +1,349 @@ +/** + * @file CurtisMotorController.cpp + * @brief 柯蒂斯电机控制器实现文件 + */ + +#include "../include/can/CurtisMotorController.h" +#include +#include +#include +#include + +// ==================== 构造函数和析构函数 ==================== + +CurtisMotorController::CurtisMotorController(DWORD deviceType, + DWORD deviceIndex, + DWORD canChannel) + : m_deviceType(deviceType) + , m_deviceIndex(deviceIndex) + , m_canChannel(canChannel) + , m_isConnected(false) + , m_statusCallback(nullptr) +{ +} + +CurtisMotorController::~CurtisMotorController() { + Disconnect(); +} + +// ==================== 连接管理 ==================== + +bool CurtisMotorController::Connect() { + if (m_isConnected) { + std::cout << "[Curtis] 警告: 已经连接" << std::endl; + return true; + } + + std::cout << "[Curtis] 正在连接 Curtis 控制器..." << std::endl; + + // 步骤1: 打开 CAN 设备 + std::cout << "[Curtis] 1/4 打开 CAN 设备 (类型=" << m_deviceType + << ", 索引=" << m_deviceIndex << ")..." << std::endl; + + DWORD ret = VCI_OpenDevice(m_deviceType, m_deviceIndex, 0); + if (ret != 1) { + std::cerr << "[Curtis] 错误: 打开设备失败 (返回值=" << ret << ")" << std::endl; + return false; + } + std::cout << "[Curtis] ✓ 设备打开成功" << std::endl; + + // 步骤2: 配置 CAN 参数（125kbps） + std::cout << "[Curtis] 2/4 初始化 CAN (125kbps)..." << std::endl; + + VCI_INIT_CONFIG config; + config.AccCode = 0x80000008; + config.AccMask = 0xFFFFFFFF; // 接收所有帧 + config.Filter = 1; // 接收标准帧和扩展帧 + config.Timing0 = 0x03; // 125 kbps + config.Timing1 = 0x1C; + config.Mode = 0; // 正常模式 + config.Reserved = 0; + + ret = VCI_InitCAN(m_deviceType, m_deviceIndex, m_canChannel, &config); + if (ret != 1) { + std::cerr << "[Curtis] 错误: 初始化 CAN 失败 (返回值=" << ret << ")" << std::endl; + VCI_CloseDevice(m_deviceType, m_deviceIndex); + return false; + } + std::cout << "[Curtis] ✓ CAN 初始化成功" << std::endl; + + // 步骤3: 启动 CAN 通道 + std::cout << "[Curtis] 3/4 启动 CAN 通道..." << std::endl; + ret = VCI_StartCAN(m_deviceType, m_deviceIndex, m_canChannel); + if (ret != 1) { + std::cerr << "[Curtis] 错误: 启动 CAN 失败 (返回值=" << ret << ")" << std::endl; + VCI_CloseDevice(m_deviceType, m_deviceIndex); + return false; + } + std::cout << "[Curtis] ✓ CAN 通道启动成功" << std::endl; + + // 步骤4: 清空缓冲区 + std::cout << "[Curtis] 4/4 清空缓冲区..." << std::endl; + VCI_ClearBuffer(m_deviceType, m_deviceIndex, m_canChannel); + std::cout << "[Curtis] ✓ 缓冲区清空完成" << std::endl; + + m_isConnected = true; + std::cout << "[Curtis] ========== 连接成功！=========" << std::endl; + std::cout << "[Curtis] 准备通讯（请以 ≤20ms 周期发送命令）" << std::endl; + + return true; +} + +void CurtisMotorController::Disconnect() { + if (!m_isConnected) { + return; + } + + std::cout << "[Curtis] 正在断开连接..." << std::endl; + + // 发送停止命令 + std::cout << "[Curtis] 发送停止命令..." << std::endl; + SendStopCommand(); + Sleep(50); // 等待命令发送 + + // 关闭设备 + std::cout << "[Curtis] 关闭 CAN 设备..." << std::endl; + VCI_CloseDevice(m_deviceType, m_deviceIndex); + + m_isConnected = false; + std::cout << "[Curtis] ✓ 断开连接完成" << std::endl; +} + +// ==================== 命令发送 ==================== + +bool CurtisMotorController::SendCommand(const CurtisCommand& cmd) { + if (!m_isConnected) { + std::cerr << "[Curtis] 错误: 未连接到控制器" << std::endl; + return false; + } + + // 限制数值范围 + int16_t driveSpeed = cmd.driveSpeed; + if (driveSpeed < CURTIS_DRIVE_SPEED_MIN) driveSpeed = CURTIS_DRIVE_SPEED_MIN; + if (driveSpeed > CURTIS_DRIVE_SPEED_MAX) driveSpeed = CURTIS_DRIVE_SPEED_MAX; + + int16_t liftLeft = cmd.liftSpeedLeft; + if (liftLeft < CURTIS_LIFT_SPEED_MIN) liftLeft = CURTIS_LIFT_SPEED_MIN; + if (liftLeft > CURTIS_LIFT_SPEED_MAX) liftLeft = CURTIS_LIFT_SPEED_MAX; + + int16_t liftRight = cmd.liftSpeedRight; + if (liftRight < CURTIS_LIFT_SPEED_MIN) liftRight = CURTIS_LIFT_SPEED_MIN; + if (liftRight > CURTIS_LIFT_SPEED_MAX) liftRight = CURTIS_LIFT_SPEED_MAX; + + int16_t steerAngle = cmd.steerAngle; + if (steerAngle < CURTIS_STEER_ANGLE_MIN) steerAngle = CURTIS_STEER_ANGLE_MIN; + if (steerAngle > CURTIS_STEER_ANGLE_MAX) steerAngle = CURTIS_STEER_ANGLE_MAX; + + // 发送帧1: 0x1E5 - 行走和提升命令 + BYTE data1[8]; + data1[0] = cmd.controlBits0; + data1[1] = cmd.controlBits1 | CURTIS_CTRL_AGV_CONNECTED; // 确保AGV标识位 + data1[2] = driveSpeed & 0xFF; + data1[3] = (driveSpeed >> 8) & 0xFF; + data1[4] = liftLeft & 0xFF; + data1[5] = (liftLeft >> 8) & 0xFF; + data1[6] = liftRight & 0xFF; + data1[7] = (liftRight >> 8) & 0xFF; + + if (!SendFrame(CURTIS_ID_DRIVE_CMD, data1, 8)) { + std::cerr << "[Curtis] 错误: 发送 0x1E5 失败" << std::endl; + return false; + } + + // 发送帧2: 0x2E5 - 转向命令 + BYTE data2[8] = {0}; + data2[0] = steerAngle & 0xFF; + data2[1] = (steerAngle >> 8) & 0xFF; + // Byte2-7 已初始化为0 + + if (!SendFrame(CURTIS_ID_STEER_CMD, data2, 8)) { + std::cerr << "[Curtis] 错误: 发送 0x2E5 失败" << std::endl; + return false; + } + + // 保存最后发送的命令 + m_lastCommand = cmd; + + return true; +} + +bool CurtisMotorController::SendStopCommand() { + CurtisCommand stopCmd; + stopCmd.controlBits0 = 0x00; // 调速开关有效 + stopCmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED; + stopCmd.driveSpeed = 0; + stopCmd.liftSpeedLeft = 0; + stopCmd.liftSpeedRight = 0; + stopCmd.steerAngle = 0; + + return SendCommand(stopCmd); +} + +void CurtisMotorController::EmergencyStop() { + if (!m_isConnected) { + return; + } + + std::cout << "[Curtis] !!! 执行紧急停车 !!!" << std::endl; + + CurtisCommand stopCmd; + stopCmd.controlBits0 = 0x00; + stopCmd.controlBits1 = CURTIS_CTRL_EMERGENCY_STOP | CURTIS_CTRL_AGV_CONNECTED; + stopCmd.driveSpeed = 0; + stopCmd.liftSpeedLeft = 0; + stopCmd.liftSpeedRight = 0; + stopCmd.steerAngle = 0; + + // 连续发送3次确保收到 + for (int i = 0; i < 3; i++) { + SendCommand(stopCmd); + Sleep(10); + } +} + +// ==================== 状态接收 ==================== + +bool CurtisMotorController::ReceiveStatus() { + if (!m_isConnected) { + return false; + } + + VCI_CAN_OBJ rxFrames[2500]; + DWORD count = VCI_Receive(m_deviceType, m_deviceIndex, + m_canChannel, rxFrames, 2500, 0); + + if (count == (DWORD)-1) { + std::cerr << "[Curtis] 错误: USB 断开或设备不存在" << std::endl; + m_isConnected = false; + return false; + } + + bool statusUpdated = false; + + for (DWORD i = 0; i < count; i++) { + if (rxFrames[i].ID == CURTIS_ID_STATUS) { + ParseStatusFrame(rxFrames[i]); + statusUpdated = true; + + // 调用回调函数 + if (m_statusCallback) { + m_statusCallback(m_status); + } + } + else if (rxFrames[i].ID == CURTIS_ID_RESERVED) { + // 0x360 保留帧，暂不处理 + } + } + + return statusUpdated; +} + +// ==================== 故障和超时检测 ==================== + +bool CurtisMotorController::IsTimeout(uint32_t timeoutMs) const { + if (m_status.lastUpdateTime == 0) { + return true; // 从未接收过数据 + } + + uint32_t currentTime = GetTickCount(); + uint32_t elapsed = currentTime - m_status.lastUpdateTime; + + return elapsed > timeoutMs; +} + +std::string CurtisMotorController::GetErrorString() const { + std::ostringstream oss; + + if (m_status.driveError != 0) { + oss << "行走故障=0x" << std::hex << (int)m_status.driveError << " "; + } + if (m_status.steerError != 0) { + oss << "转向故障=0x" << std::hex << (int)m_status.steerError << " "; + } + if (m_status.isEmergencyStop) { + oss << "[急停] "; + } + + std::string result = oss.str(); + return result.empty() ? "无故障" : result; +} + +// ==================== 工具函数 ==================== + +int16_t CurtisMotorController::SpeedPercentToCAN(float percent) { + if (percent > 100.0f) percent = 100.0f; + if (percent < -100.0f) percent = -100.0f; + return static_cast(percent * 40.95f); +} + +float CurtisMotorController::SpeedCANToPercent(int16_t canValue) { + return canValue / 40.95f; +} + +int16_t CurtisMotorController::AngleDegreeToCAN(float degrees) { + if (degrees > 90.0f) degrees = 90.0f; + if (degrees < -90.0f) degrees = -90.0f; + return static_cast(degrees * 10.0f); +} + +float CurtisMotorController::AngleCANToDegree(int16_t canValue) { + return canValue / 10.0f; +} + +void CurtisMotorController::PrintStatus() const { + std::cout << "\n========== Curtis 状态 ==========" << std::endl; + std::cout << "行走故障: 0x" << std::hex << (int)m_status.driveError + << " 转向故障: 0x" << (int)m_status.steerError << std::dec << std::endl; + std::cout << "转向角度: " << m_status.currentAngle << "°" << std::endl; + std::cout << "电机转速: " << m_status.motorSpeed << std::endl; + std::cout << "电量: " << (int)m_status.batteryLevel << "%" << std::endl; + std::cout << "模式: " << (m_status.isAutoMode ? "自动" : "手动") + << " 急停: " << (m_status.isEmergencyStop ? "是" : "否") << std::endl; + std::cout << "================================\n" << std::endl; +} + +// ==================== 内部辅助函数 ==================== + +bool CurtisMotorController::SendFrame(UINT canId, const BYTE* data, BYTE len) { + VCI_CAN_OBJ frame; + memset(&frame, 0, sizeof(VCI_CAN_OBJ)); + + frame.ID = canId; + frame.SendType = 0; // 正常发送（自动重试） + frame.RemoteFlag = 0; // 数据帧 + frame.ExternFlag = 0; // 标准帧 + frame.DataLen = len; + + if (len > 8) len = 8; + memcpy(frame.Data, data, len); + + DWORD sent = VCI_Transmit(m_deviceType, m_deviceIndex, + m_canChannel, &frame, 1); + + return sent == 1; +} + +void CurtisMotorController::ParseStatusFrame(const VCI_CAN_OBJ& frame) { + // Byte0: 行走故障代码 + m_status.driveError = frame.Data[0]; + + // Byte1: 转向故障代码 + m_status.steerError = frame.Data[1]; + + // Byte2-3: 转向角度反馈 + int16_t angleRaw = static_cast((frame.Data[3] << 8) | frame.Data[2]); + m_status.currentAngle = angleRaw / 10.0f; + + // Byte4-5: 电机转速 + m_status.motorSpeed = static_cast((frame.Data[5] << 8) | frame.Data[4]); + + // Byte6: 电量 + m_status.batteryLevel = frame.Data[6]; + + // Byte7: 状态位 + m_status.isAutoMode = (frame.Data[7] & CURTIS_STATUS_AUTO_MODE) != 0; + m_status.isEmergencyStop = (frame.Data[7] & CURTIS_STATUS_EMERGENCY_STOP) != 0; + + // 更新时间戳 + m_status.lastUpdateTime = GetTickCount(); +}