284 lines
8.5 KiB
C++
284 lines
8.5 KiB
C++
/**
|
||
* @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;
|
||
}
|