Мобильное роботизированное (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

Я наконец понял, что не так, это потому, что я не мог выйти из цикла while, и робот не может остановить поиск ориентации. Спасибо за вдохновение.

Answer 2

Я не знаю, каково содержание вашего og структура, поэтому при условии, что есть члены, которые предоставляют информацию о текущей позиции, (я предполагаю, что они posx & posy), вы должны иметь тестовый оператор сразу после прочтения последней позиции, что-то вроде этого:

[EDIT] переустановил 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