core function of the road identification algorithm

offsetFinder.c

float offsetFinder(void){
    int i,j;
    int offset_counter = 0;
    int bottom_edge_counter = 0;
    float sum = 0;

    
    //Step 1: 从摄像头获取的赛道图像的下方，向上寻找黑色赛道边界。非边界点为－1
    //printf("\nedge_line:");
    for(j=0;j<HORIZONTAL_PIXEL_NUM;j++){
        for(i=0;i<SAMPLE_COL;i++){
            if(AD_Sample[SAMPLE_COL-1-i][j] == 1){
                edge_line[j] = SAMPLE_COL-1-i;
                //如果边界点位于赛道图像下半部分，则bottom_edge_counter加一
                if(2*edge_line[j] > VERTICAL_PIXEL_NUM)bottom_edge_counter++;
                break;
            }
            //如果未找到黑色赛道边界，就赋值－1
            if((i==SAMPLE_COL-1)&&(AD_Sample[SAMPLE_COL-1-i][j] == 0))edge_line[j] = -1;
        }
        //printf(" %i ",edge_line[j]);
    }
    //Sample data: 40 38 38 20 4 -1 -1 -1 -1 -1 6 14 20 25
 

    //Step 2: 求边界点序列中每相邻两点竖直方向的偏移量。无效点为BAD_POINT
    //printf("\nvert offset:");
    for(j=0;j<HORIZONTAL_PIXEL_NUM;j++){
        if(edge_line[j] != -1){            
            if((j != 0)&&(edge_line[j-1] != -1 )) offset[j] = edge_line[j] - edge_line[j-1];
            
            //每段横向坐标连续的边界点序列的第一个点的偏移量无效
            else offset[j] = BAD_POINT;
        }
        //未找到黑色赛道边界的点无效
        else offset[j] = BAD_POINT;
        //printf(" %f ",offset[j]);
    }
    //Sample data: X -2 0 -18 -16 X X X X X X 8 6 5
    
    //Step 3: 将所有有效竖向偏移量（去掉BAD_POINT和0）转换为横向偏移量pure_offset，并求和sum。横向偏移量才是赛车需要的角度调整依据。
    //printf("\nhorizontal offset and sum:");
    for(j=0;j<HORIZONTAL_PIXEL_NUM;j++){
        if((offset[j]!= BAD_POINT)&&(offset[j] != 0)){
            //竖向偏移量为0的点不管
            pure_offset[offset_counter] = 1 / offset[j]; 
            sum += pure_offset[offset_counter];
            offset_counter++;
            //printf(" %f (%f) ", offset[j], sum);
        }
    }
    //Sample data:   -2   -18 -16             8 6 5
    
    //Step 4:根据仅含正负横向偏移量的序列pure_offset，判断前方路况，最终求出平均偏移量
    //各种路况的典型数据:
    //直线 ---+++
    //右转弯 ------
    //左转弯 ++++++
    //十字路口 +++---
    //出界 -+--+-++-+
    
    //最先检测是否出界
    /*
    for(j=0;j<offset_counter-1;j++){
	//记录正负交界的次数来判断是否出界
        if(pure_offset[j] * pure_offset[j+1] < 0) deflection_counter++;
	      if(deflection_counter > 10){
		    //与上次转弯同方向，打死急转
	         	if(last_turn > 0) return average_offset = 0.5;
	        	if(last_turn < 0) return average_offset = -0.5;
	      } 
    }  */
    
        //检测是否压线 
    for(j=0,offline_counter=0;j<20;j++){
	      if(edge_line[j] == SAMPLE_COL-1) offline_counter++;
        if(offline_counter > 14) return average_offset = -3;
    } 
    for(j=HORIZONTAL_PIXEL_NUM-20,offline_counter=0;j<HORIZONTAL_PIXEL_NUM;j++){
      	if(edge_line[j] == SAMPLE_COL-1) offline_counter++;
        if(offline_counter > 14) return average_offset = 3;
    }
    
    //检测十字路口
    for(j=0;j<offset_counter-1;j++){
        //检测是否为十字路口（检测出界时已排除多个转折的可能）
        if((pure_offset[j]>0)&&(pure_offset[j+1]<0)){
            //判断哪一条是目标路径（横向偏移量绝对值较小的为目标路径，即前进的直线）
            if(pure_offset[j] - (-1*pure_offset[j+1]) < 0) return average_offset = pure_offset[j];
            else return average_offset = pure_offset[j+1];
        }
    }

    //其他路况均通过求平均值得出平均偏移量值
    average_offset = sum / offset_counter ;
    return average_offset;
}