mobile robotic (e-puck) programming (c language)

Question

i am programming my differential drive mobile robot (e-puck) to move to a certain coordinate with specific orientation. the robot has no problem reaching the coordinate, however when it reaches the coordinate, it cannot settle on the specific orientation and keeping "spinning" on the spot searching for the orientation, do anyone have any prior experience in this? i am stuck in this issue for very long and really hope someone knows why. the relevant part of the code is pasted below.

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

As a side note which might be relevant: why do you set `spee_left` and `speed_right` to zero, and then immediately check if they are zero?! Anyway, the problem seems to be with calculation of `og.result.atgoal`. Either your definition of "close enough" is too precise for the robot to achieve, or someone is resetting `og.result.atgoal` back to zero everytime. Since the `if` I mentioned above is always true, `og.result.atgoal` should always immediately become 1, so I highly suspect it gets reinitialized to zero somewhere else. — Shahbaz, Nov 04 '13 at 14:10
Where in the while loop are you getting feedback. i.e., you have a setpoint (a desired location), what function contains how close you are to that location? Is it `odometry_goto_step(&og);` by chance? Also, can you please post the definition of the `og` struct? I assume there are position variables, perhaps such as posx, posy? Seeing whats there would help to make suggestions. — ryyker, Nov 04 '13 at 15:03

ryyker · Answer 1 · 2013-11-04T15:27:05.297

I do not have the benefit of knowing what the contents of your og struct are, so assuming there are members that provide current position information, (I will assume they are posx & posy), you should should have a test statement sometime just after having read the latest position, something like this:

[EDIT] re-positioned 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

score 0 · Answer 2 · answered Nov 04 '13 at 17:50

0

i finally figured out what is wrong, it is because i could not get out of the while loop and the robot cannot stop searching for the orientation. Thanks for the inspiration.

answered Nov 04 '13 at 17:50

user168124

21
2

I can see you are new to stackoverflow. Welcome! In the future you should know to edit the _original post_ with additional information about your question. It is also appropriate to ***answer your own question***, _but when you do_, mark it as answered to close the question. (There is a check-mark symbol next to all of the answers visible only to the original poster, mark the answer that satisfies your question, yours in this case) – ryyker Nov 05 '13 at 00:45

mobile robotic (e-puck) programming (c language)

2 Answers2