移动机器人(e-puck)编程(c语言)

Question

我正在对我的差动驱动移动机器人(e-puck)进行编程，使其移动到具有特定方向的特定坐标。机器人到达坐标没有问题，但是当它到达坐标时，它不能确定具体的方向，并在现场“旋转”寻找方向，有人在这方面有经验吗？我被困在这个问题上很长时间了，真的希望有人知道为什么。下面粘贴了代码的相关部分。

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;
}

Answer 1

我没有好处知道你的og结构的内容是什么，所以假设有提供当前位置信息的成员(我假设他们是posx & posy)，你应该在阅读完最新位置后的某个时候有一个测试语句，如下所示：

编辑重新定位的set_speed()

while (og.result.atgoal == 0) 
{
    // Update position and calculate new speeds
    odometry_track_step(og.track);
    odometry_goto_step(&og);

    if(((og.result.posx - x) > 3) || (og.result.posy - y) > 3)    //(distance assumed in mm)
    {
       //then report position, continue to make adjustments and navigate
        printf("%f",ot.result.theta);
        print_position();
        // Set the wheel speed
        set_speed(og.result.speed_left, og.result.speed_right);
        step();

    }
    else
    {
            //set all speeds to 0
            //report position.          
            //clear appropriate struct members and leave loop
    }

} //after exiting while loop speed will be zero

Answer 2

我终于找出了问题所在，因为我无法走出while循环，机器人无法停止搜索方向。谢谢你的启发。