limited range detection part of our robot

limitedrangedet.cpp

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