[Playerstage-users] Avoidance / Wandering

[Hunter A. <hun...@gm...>]

Hi. I want some help with two things. I am trying to write a way to wander. I
am using the Sick laser with a pioneer. Here is my Avoid method:

void Robot::Avoidance2()
{
    list<double> group1, group2, group3, group4;
    int numPer = laserProxy->GetCount() / 4;
    const double MIN_D = .5;
	const double TURN_SPEED = 40;

    for (int x = 0; x < numPer; x++)
    {
        group1.push_back(laserProxy->GetRange(x));
        group2.push_back(laserProxy->GetRange(x + numPer));
        group3.push_back(laserProxy->GetRange(x + 2 * numPer));
        group4.push_back(laserProxy->GetRange(x + 3 * numPer));
    }//fill laser readings in the lists.

    float percent1 = getPercent(group1);
    float percent2 = getPercent(group2);
    float percent3 = getPercent(group3);
    float percent4 = getPercent(group4);

    float preLeft  = percent1 + percent2;
    float preRight = percent3 + percent4;
    double normalizer = pow(preLeft + preRight, -1);
    float left = preLeft * normalizer;
    float right = preRight * normalizer;//get a percentage per side. Just a
comparison value.

    int direction = getDirection(left, right);

    if (percent2 > .5 && percent3 > .5)
    {
        if (percent1 > .4 && percent4 > .4)
        {
            forwardSpeed = -0.5;
            turnSpeed = 0;
        }//no space on left and right.

        else if (percent1 > .25 && !percent4 > .25)
        {
            forwardSpeed = -0.3;
            turnSpeed = TURN_SPEED / 2;
        }//no space on left

        else if (percent4 > .25 && !percent1 > .25)
        {
            forwardSpeed = -0.3;
            turnSpeed = -TURN_SPEED / 2;
        }//no space on right
    }//end if.

    else if (percent1 > .4 && percent2 > .4)
    {
        forwardSpeed = 0;
        turnSpeed = -TURN_SPEED;
    }

    else if (percent1 > .4 && percent4 > .4)
    {
        if (forwardSpeed > 0.1)
            forwardSpeed -= 0.1;
    }

    else
    {
        if (left < right)
                turnSpeed -= right - left;
        else if (right < left)
                turnSpeed += left - right;
        else
        {
            turnSpeed = 0;
            forwardSpeed = 0.5;
        }//end else
    }//end else
}//end void.

The issue I am having seems to be with the if statements. If necessary, I
can attach the rest of the stuff. I am running with stage in a map of the
building in which I work. I can post all this stuff. The other question I
have has to do with the wavefront driver. I have no idea what I'm doing with
it... Could I acquire some sample code?
-- 
View this message in context: http://old.nabble.com/Avoidance---Wandering-tp31768641p31768641.html
Sent from the playerstage-users mailing list archive at Nabble.com.