Skip to content

Instantly share code, notes, and snippets.

Created Jun 17, 2016
Embed
What would you like to do?
limited range detection part of our robot
detection::detection(std::vector<float> ranges, int vecsize, emc::OdometryData odom, double anglerad, double angleincr)
{
//depedent rotation
int n;
int m;
double x [vecsize];
double y [vecsize];
float maxrange=1.5;
int count;
std::vector<float> frange=ranges;
int refdata[vecsize];
int pt1;
int pt2;
int nrpoint;
//==================max range mod===============
//this part count all values that are smaller than the initial max range.
count=0;
for( n=0;n<vecsize;n++)
{
if (ranges[n]<maxrange)
{
count++;
}
}
if (count>600) // if 60% or more of all values are smaller than the inital maxrange, the max range is reduced.
{
maxrange=1.1;
}
//=====================end max range mod
//the LRF data is limited to max range in this part
for( n=0;n<vecsize;n++)
{
if (ranges[n]>maxrange)
{
ranges[n]=maxrange;
}
else if(ranges[n]<0.01) // if Pico doesnt see anything it give a small number, this is a failsafe for that.
{
ranges[n]=maxrange;
}
x[n] = sin(anglerad)*ranges[n]; // x and y values are calculated for each element in the ranges vector
y[n] =cos(anglerad)*ranges[n];
anglerad -= angleincr;
}
//===============detection ref========================
// in this part the open spaces are determined and the element of the ranges vector corresponding to the reference point is stored in an array.
if (ranges[0]==maxrange)
{
ranges[0]=maxrange-0.01;
}
else if (ranges[999]==maxrange)
{
ranges[999]=maxrange-0.01;
}
count=0;
nrpoint=0;
for( n=1;n<vecsize;n++)
{
if (ranges[n]==maxrange && ranges[n-1] != maxrange)
{
pt1=n;
count++;
}
else if (ranges[n]==maxrange)
{
count++;
}
else if (ranges[n]!=maxrange && ranges[n-1]==maxrange)
{
if (count>150) //the open space has to be minimal 150 points long in order to be seen as an open space
{
pt2=n;
refdata[nrpoint]=(pt1+pt2)/2;
nrpoint++;
count=0;
}
else
{
count=0;
}
}
}
for(n=nrpoint;n<10;n++)
{
refdata[n]=0;
}
if (nrpoint==0) //If no open spaces are detected, the reference element is 500, which corresponds with right in front of the robot.
{
refdata[0]=500;
}
//the data is being stored in classes
detection::refx = x[refdata[0]];
detection::refy = y[refdata[0]];
detection::ref1x = x[refdata[1]];
detection::ref1y = y[refdata[1]];
detection::ref2x = x[refdata[2]];
detection::ref2y = y[refdata[2]];
detection::nrop=nrpoint;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment