柯蒂斯控制协议，相关集成
This commit is contained in:
CaiXiang
283
examples/curtis_path_tracking_demo.cpp
Normal file
283
examples/curtis_path_tracking_demo.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user