Skip to content

Instantly share code, notes, and snippets.

Created Jun 17, 2016
Embed
What would you like to do?
planning of our robot
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;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment