planning of our robot

planning.cpp

planning::planning(detection data2, int turn, double odomy, double odoma)
{


//========================================Semi right hand strategy==================================


std::cout<<"odom.a data = "<<odoma<<std::endl;


int corner=0;
float oneandhalfrot=-9;
float twoandhalfrot=-16;
float resetrot1=-1;
float resetrot2=-10;

//Solving stategy for the maze. At first the robot will always take the most right reference point,
//however, when a certain amount of clockwise rotation is passed, it will take the second reference point, if available. After even
//more clockwise rotation the robot will take the last reference point available.

//below is the code to determine which reference point to pick at the next crossing.
if(odoma>0)
{
    cstate=0;                                               //state for which the most right reference point is preferred.
}                                                           //This is our choice making state.

if(odoma<oneandhalfrot && odoma>twoandhalfrot &&cstate==0)  //When this argument is satisfied the choice making state changes
{
    cstate=1;                                               //state for which the second reference point is preferred
}
if (odoma<twoandhalfrot&&cstate==1)                         //When this argument is satisfied the choice making state changes again.
{
    cstate=2;                                               //state for which the last reference point is preferred
}

if (cstate==1&& odoma>resetrot1)                            //When this argument is satisfied the choice making state resets to
{                                                           //the most right reference point again.
    cstate=0;
}
if (cstate==2&& odoma>resetrot2)                            //This argument resets the ref point preference from the last ref point to second.
{
    cstate=1;
}

//below is the code that couples the preferred reference point to the options that are available.
//A corner state of 0 means that the most right option is preferred, which is always available. This is
// each loop the inital state of corner. if the corner state is 1, it will directly pick the second reference point,
//and when it is 2 it will directly pick the third reference point.
//The difference between the choice making state and the corner state is that the choice state gives a preference, while
//the corner state is based onthe combination of the available options and the preferred state.
if (cstate==1)                                              //When the choice state state is 1, this argument check wether there are more than
{                                                           //one options available. If this is the case, it will pick the second ref point.
    if(data2.nrop>1)                                        //This state gives the actual number of options available.
    {
    corner=1;
    }
}
if (cstate==2)                                              // when the choice state is 2, this argument check wether there are more than
{                                                           //one options available. if there are two options, it will pick the second ref point,
    if(data2.nrop==2)                                       //and when there are 3 options, it will pick the third reference ref point.
    {
    corner=1;
    }
    else if(data2.nrop>2)
    {
    corner=2;
    }
}


//=========================================end RH strategy================================
if (data2.ldoor==3||data2.rdoor==3)
{
    if (data2.nrop==3)
    {
        corner=1;
    }

    else if (data2.nrop==2)
    {
        corner=1;
    }

}

//===================================left right or straight====================
//corner==0 is most right
//corner==1 is 1 left of the most right path
//corner==2 is 2 left of the most right path
if (corner==1)
{
    planning::refx=data2.ref1x;
    planning::refy=data2.ref1y;
}

else if (corner==2)
{
    //corner=0;
    planning::refx=data2.ref2x;
    planning::refy=data2.ref2y;
}

else if (corner==0)
{
    //corner=0;
    planning::refx=data2.refx;
    planning::refy=data2.refy;
}
}