if (! paths[i].empty()) { double cur_x = agvs[i].current_x; double cur_y = agvs[i].current_y; Node* next_node = paths[i][0]; float speed = agvs[i].getSpeed(); float distance = sqrt(pow(next_node->x - cur_x, 2) + pow(next_node->y - cur_y, 2)); int steps = ceil(distance / (speed * 0.015)); // 计算需要移动的步数 float power_consumption = distance / 20; //_MAP[cur_x][cur_y] = 1; for (int j = 1; j <= steps; j++) { float time = j *100 / speed; // 每步所需时间 QTimer::singleShot(time, this, &, i, next_node, cur_x, cur_y, power_consumption, j, steps { // if (j == 1) { // //离开当前位置时将标记设为0 // //MAP[cur_x][cur_y] = 0; // } float ratio = static_cast<float>(j) / steps; double new_x = cur_x + ratio * (next_node->x - cur_x); double new_y = cur_y + ratio * (next_node->y - cur_y); agvs[i].setCurrentX(new_x); agvs[i].setCurrentY(new_y); std:: cout <<"AGV"<<"("<< new_x << "," << new_y << ")" << endl; // 更新电量 agvs[i].setpower(agvs[i].power - power_consumption / steps); this->update(); // 在窗口中重绘 }); } },AGV(1,3.92857) AGV(1,4) AGV(1,4) AGV(1,3.92857) AGV(1,4) AGV(1,3.92857) AGV(1,4) AGV(1,4) AGV(1,4) AGV(1,4) AGV(1,4.07143) AGV(1,4) AGV(1,4) AGV(1,4) AGV(1,4) AGV(1,4) AGV(1,4.07143) AGV(1,4.14286)小車隨著new_的坐標移動,會出現倒退的情況,怎麽修改代碼
时间: 2024-01-31 16:04:28 浏览: 96
ESXi5.X安装DELL_Equallogic多路径软件.docx
出现小车倒退的情况,是因为在计算新的小车位置时,使用了该小车当前位置和目标位置之间的线性插值,但是插值的过程中没有考虑小车当前的方向,导致小车有可能会“后退”。
要解决这个问题,可以在计算小车新位置时,同时计算小车前进方向的向量,然后根据该向量来判断小车是前进还是后退,进而更新小车的位置。
修改代码如下:
```
if (! paths[i].empty()) {
double cur_x = agvs[i].current_x;
double cur_y = agvs[i].current_y;
Node* next_node = paths[i][0];
float speed = agvs[i].getSpeed();
float distance = sqrt(pow(next_node->x - cur_x, 2) + pow(next_node->y - cur_y, 2));
int steps = ceil(distance / (speed * 0.015)); // 计算需要移动的步数
float power_consumption = distance / 20; //_MAP[cur_x][cur_y] = 1;
// 计算小车前进方向的向量
double dx = next_node->x - cur_x;
double dy = next_node->y - cur_y;
double norm = sqrt(dx * dx + dy * dy);
dx /= norm;
dy /= norm;
for (int j = 1; j <= steps; j++) {
float time = j * 100 / speed; // 每步所需时间
QTimer::singleShot(time, this, &, i, next_node, cur_x, cur_y, power_consumption, j, steps {
// if (j == 1) {
// //离开当前位置时将标记设为0
// //MAP[cur_x][cur_y] = 0;
// }
// 根据当前位置和前进方向计算新位置
double new_x = cur_x + dx * j * distance / steps;
double new_y = cur_y + dy * j * distance / steps;
agvs[i].setCurrentX(new_x);
agvs[i].setCurrentY(new_y);
std::cout << "AGV" << "(" << new_x << "," << new_y << ")" << endl;
// 更新电量
agvs[i].setpower(agvs[i].power - power_consumption / steps);
this->update(); // 在窗口中重绘
});
}
}
```
在计算小车新位置时,我们先计算出小车前进方向的向量,然后根据该向量来计算小车的新位置。这样就可以避免小车出现倒退的问题了。
阅读全文