233 lines
7.5 KiB
C++
233 lines
7.5 KiB
C++
/**
|
||
* @file curtis_demo.cpp
|
||
* @brief Curtis 电机控制器键盘控制演示程序
|
||
*
|
||
* 功能说明：
|
||
* - 演示 Curtis 电机控制器的完整使用流程
|
||
* - 提供键盘控制接口（WASD + QE + 空格 + ESC）
|
||
* - 实时显示控制器状态
|
||
* - 故障和急停检测
|
||
*
|
||
* 编译：
|
||
* cd build
|
||
* cmake ..
|
||
* make curtis_demo
|
||
*
|
||
* 运行：
|
||
* ./curtis_demo
|
||
*/
|
||
|
||
#include "../include/can/CurtisMotorController.h"
|
||
#include <iostream>
|
||
#include <windows.h>
|
||
#include <conio.h>
|
||
|
||
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;
|
||
}
|