CODEWITHSUNDEEP
cwebots
user168124
user168124

Reputation: 21

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

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

Upvotes: 2

Views: 1073

Answers (2)

user168124
user168124

Reputation: 21

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.

Upvotes: 0

ryyker
ryyker

Reputation: 23236

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

Upvotes: 0

Related Questions