agv/RCS-3000
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

柯蒂斯控制协议，相关集成

Browse Source
This commit is contained in:
CaiXiang
2025-11-15 16:05:59 +08:00
parent ec1d6f0cee
commit c34b5dc853
7 changed files with 3075 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

232
examples/curtis_demo.cpp Normal file
View File

@@ -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 <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;
}

283
examples/curtis_path_tracking_demo.cpp Normal file
View File

@@ -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 <iostream>
#include <vector>
#include <cmath>
#include <windows.h>
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<PathPoint> generateRectanglePath() {
vector<PathPoint> 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<PathPoint> generateCirclePath(double radius = 3.0, int points = 36) {
vector<PathPoint> 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<PathPoint>& 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<PathPoint> 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;
}