修复柯蒂斯控制器依赖引入bug
This commit is contained in:
@@ -8,7 +8,9 @@
|
|||||||
"Bash(test -f:*)",
|
"Bash(test -f:*)",
|
||||||
"Bash(mkdir:*)",
|
"Bash(mkdir:*)",
|
||||||
"Bash(chmod:*)",
|
"Bash(chmod:*)",
|
||||||
"Bash(./scripts/archive_bug_fix.sh:*)"
|
"Bash(./scripts/archive_bug_fix.sh:*)",
|
||||||
|
"Bash(cmake --build:*)",
|
||||||
|
"Bash(cmake:*)"
|
||||||
],
|
],
|
||||||
"deny": [],
|
"deny": [],
|
||||||
"ask": []
|
"ask": []
|
||||||
|
|||||||
@@ -29,29 +29,29 @@
|
|||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 路径点结构
|
* @brief 简单路径点结构(用于演示)
|
||||||
*/
|
*/
|
||||||
struct PathPoint {
|
struct SimplePathPoint {
|
||||||
double x;
|
double x;
|
||||||
double y;
|
double y;
|
||||||
double targetSpeed; ///< 目标速度百分比
|
double targetSpeed; ///< 目标速度百分比
|
||||||
|
|
||||||
PathPoint(double x_, double y_, double speed_ = 30.0)
|
SimplePathPoint(double x_, double y_, double speed_ = 30.0)
|
||||||
: x(x_), y(y_), targetSpeed(speed_) {}
|
: x(x_), y(y_), targetSpeed(speed_) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 生成矩形路径
|
* @brief 生成矩形路径
|
||||||
*/
|
*/
|
||||||
vector<PathPoint> generateRectanglePath() {
|
vector<SimplePathPoint> generateRectanglePath() {
|
||||||
vector<PathPoint> path;
|
vector<SimplePathPoint> path;
|
||||||
|
|
||||||
// 5m x 3m 矩形
|
// 5m x 3m 矩形
|
||||||
path.push_back(PathPoint(0.0, 0.0, 30.0)); // 起点
|
path.push_back(SimplePathPoint(0.0, 0.0, 30.0)); // 起点
|
||||||
path.push_back(PathPoint(5.0, 0.0, 30.0)); // 直行5米
|
path.push_back(SimplePathPoint(5.0, 0.0, 30.0)); // 直行5米
|
||||||
path.push_back(PathPoint(5.0, 3.0, 25.0)); // 右转前进3米
|
path.push_back(SimplePathPoint(5.0, 3.0, 25.0)); // 右转前进3米
|
||||||
path.push_back(PathPoint(0.0, 3.0, 25.0)); // 左转前进5米
|
path.push_back(SimplePathPoint(0.0, 3.0, 25.0)); // 左转前进5米
|
||||||
path.push_back(PathPoint(0.0, 0.0, 20.0)); // 返回起点
|
path.push_back(SimplePathPoint(0.0, 0.0, 20.0)); // 返回起点
|
||||||
|
|
||||||
return path;
|
return path;
|
||||||
}
|
}
|
||||||
@@ -59,14 +59,14 @@ vector<PathPoint> generateRectanglePath() {
|
|||||||
/**
|
/**
|
||||||
* @brief 生成圆形路径
|
* @brief 生成圆形路径
|
||||||
*/
|
*/
|
||||||
vector<PathPoint> generateCirclePath(double radius = 3.0, int points = 36) {
|
vector<SimplePathPoint> generateCirclePath(double radius = 3.0, int points = 36) {
|
||||||
vector<PathPoint> path;
|
vector<SimplePathPoint> path;
|
||||||
|
|
||||||
for (int i = 0; i <= points; i++) {
|
for (int i = 0; i <= points; i++) {
|
||||||
double angle = 2.0 * M_PI * i / points;
|
double angle = 2.0 * M_PI * i / points;
|
||||||
double x = radius * cos(angle);
|
double x = radius * cos(angle);
|
||||||
double y = radius * sin(angle);
|
double y = radius * sin(angle);
|
||||||
path.push_back(PathPoint(x, y, 25.0));
|
path.push_back(SimplePathPoint(x, y, 25.0));
|
||||||
}
|
}
|
||||||
|
|
||||||
return path;
|
return path;
|
||||||
@@ -76,7 +76,7 @@ vector<PathPoint> generateCirclePath(double radius = 3.0, int points = 36) {
|
|||||||
* @brief 执行路径跟踪
|
* @brief 执行路径跟踪
|
||||||
*/
|
*/
|
||||||
bool executePathTracking(CurtisMotorController& curtis,
|
bool executePathTracking(CurtisMotorController& curtis,
|
||||||
const vector<PathPoint>& path,
|
const vector<SimplePathPoint>& path,
|
||||||
AGVModel& agv,
|
AGVModel& agv,
|
||||||
PathTracker& tracker) {
|
PathTracker& tracker) {
|
||||||
if (path.empty()) {
|
if (path.empty()) {
|
||||||
@@ -124,12 +124,7 @@ bool executePathTracking(CurtisMotorController& curtis,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 获取 AGV 当前状态(这里使用模拟数据,实际应用中应从传感器获取)
|
// 获取 AGV 当前状态(这里使用模拟数据,实际应用中应从传感器获取)
|
||||||
State currentState;
|
AGVModel::State currentState = agv.getState();
|
||||||
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 dx = targetX - currentState.x;
|
||||||
@@ -173,7 +168,7 @@ bool executePathTracking(CurtisMotorController& curtis,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 更新 AGV 模型(模拟)
|
// 更新 AGV 模型(模拟)
|
||||||
ControlInput control;
|
AGVModel::Control control;
|
||||||
control.v = actualSpeed / 100.0;
|
control.v = actualSpeed / 100.0;
|
||||||
control.delta = steerAngleDeg * M_PI / 180.0;
|
control.delta = steerAngleDeg * M_PI / 180.0;
|
||||||
agv.update(control, 0.015); // 15ms
|
agv.update(control, 0.015); // 15ms
|
||||||
@@ -219,16 +214,14 @@ int main() {
|
|||||||
|
|
||||||
// 创建 AGV 模型(用于模拟)
|
// 创建 AGV 模型(用于模拟)
|
||||||
AGVModel agv;
|
AGVModel agv;
|
||||||
State initialState;
|
AGVModel::State initialState;
|
||||||
initialState.x = 0.0;
|
initialState.x = 0.0;
|
||||||
initialState.y = 0.0;
|
initialState.y = 0.0;
|
||||||
initialState.theta = 0.0;
|
initialState.theta = 0.0;
|
||||||
initialState.v = 0.0;
|
|
||||||
initialState.omega = 0.0;
|
|
||||||
agv.setState(initialState);
|
agv.setState(initialState);
|
||||||
|
|
||||||
// 创建路径跟踪器
|
// 创建路径跟踪器
|
||||||
PathTracker tracker;
|
PathTracker tracker(agv);
|
||||||
|
|
||||||
// ============ 2. 连接 ============
|
// ============ 2. 连接 ============
|
||||||
cout << "\n步骤 2: 连接到 Curtis 控制器..." << endl;
|
cout << "\n步骤 2: 连接到 Curtis 控制器..." << endl;
|
||||||
@@ -248,7 +241,7 @@ int main() {
|
|||||||
int choice;
|
int choice;
|
||||||
cin >> choice;
|
cin >> choice;
|
||||||
|
|
||||||
vector<PathPoint> path;
|
vector<SimplePathPoint> path;
|
||||||
if (choice == 1) {
|
if (choice == 1) {
|
||||||
path = generateRectanglePath();
|
path = generateRectanglePath();
|
||||||
cout << "已选择: 矩形路径" << endl;
|
cout << "已选择: 矩形路径" << endl;
|
||||||
|
|||||||
@@ -79,10 +79,16 @@ public:
|
|||||||
double getMaxVelocity() const { return max_velocity_; }
|
double getMaxVelocity() const { return max_velocity_; }
|
||||||
double getMaxSteeringAngle() const { return max_steering_angle_; }
|
double getMaxSteeringAngle() const { return max_steering_angle_; }
|
||||||
|
|
||||||
|
// 状态管理
|
||||||
|
void setState(const State& state) { state_ = state; }
|
||||||
|
const State& getState() const { return state_; }
|
||||||
|
void update(const Control& control, double dt) { state_ = update(state_, control, dt); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
double wheelbase_; // 轮距 (m)
|
double wheelbase_; // 轮距 (m)
|
||||||
double max_velocity_; // 最大速度 (m/s)
|
double max_velocity_; // 最大速度 (m/s)
|
||||||
double max_steering_angle_; // 最大转向角 (rad)
|
double max_steering_angle_; // 最大转向角 (rad)
|
||||||
|
State state_; // 当前状态
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // AGV_MODEL_H
|
#endif // AGV_MODEL_H
|
||||||
|
|||||||
Reference in New Issue
Block a user