我正在编程我的差分驱动移动机器人(电子球)移动到特定方向的特定坐标。机器人到达坐标没有问题,但是当它到达坐标时,它不能定位在特定的方向上,并在现场保持“旋转”寻找方向,有没有人有过这方面的经验?我被困在这个问题很长时间,真的希望有人知道为什么。代码的相关部分粘贴在下面。移动机器人(电子球)编程(c语言)
static void step() {
if (wb_robot_step(TIME_STEP)==-1) {
wb_robot_cleanup();
exit(EXIT_SUCCESS);
}
}
.
.
.
.
.
static void set_speed(int l, int r)
{
speed[LEFT] = l;
speed[RIGHT] = r;
if (pspeed[LEFT] != speed[LEFT] || pspeed[RIGHT] != speed[RIGHT]) {
wb_differential_wheels_set_speed(speed[LEFT], speed[RIGHT]);
}
}
.
.
.
.
static void goto_position1(float x, float y, float theta)
{
if (VERBOSE > 0) printf("Going to (%f, %f, %f)\n",x,y,theta);
// Set a target position
odometry_goto_set_goal(&og, x, y, theta);
// Move until the robot is close enough to the target position
while (og.result.atgoal == 0) {
// Update position and calculate new speeds
odometry_track_step(og.track);
odometry_goto_step(&og);
// Set the wheel speed
set_speed(og.result.speed_left, og.result.speed_right);
printf("%f",ot.result.theta);
print_position();
step();
} //after exiting while loop speed will be zero
og.result.speed_left = 0;
og.result.speed_right = 0;
if (((og.result.speed_left == 0) && (og.result.speed_right == 0)))
og.result.atgoal = 1;
return;
}
.
.
.
int main(){
//initialisation
while (wb_robot_step(TIME_STEP) != -1) {
goto_position1(0.0000000001,0.00000000001,PI/4);
}
return 0;
}
+1
作为一个可能相关的附注:为什么你将'spee_left'和'speed_right'设置为零,然后立即检查它们是否为零?无论如何,这个问题似乎是与'og.result.atgoal'的计算。无论您对“足够接近”的定义是否太精确,机器人都无法实现,或者有人将“og.result.atgoal”每次重置为零。由于我上面提到的'if'总是正确的,所以'og.result.atgoal'应该总是立即变为1,所以我高度怀疑它在其他地方被重新初始化为零。 –
+0
哪里在while循环中获得反馈。即你有一个设定值(一个所需的位置),那个函数包含你离那个位置有多近?它是'odometry_goto_step(&og);'偶然吗?还有,你可以发布'og'结构的定义吗?我假设有位置变量,也许像posx,posy?看到什么会有助于提出建议 –