2

我正在对我的差动驱动移动机器人（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;
}
4

2 回答 2

0

我终于弄清楚出了什么问题，这是因为我无法摆脱 while 循环，机器人无法停止搜索方向。谢谢你的灵感。

于 2013-11-04T17:50:59.563 回答
0

我不知道你的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
于 2013-11-04T15:21:03.580 回答