智能车竞赛圆环、避障等元素方案开源

一、圆环

1.基于传统扫线的补线方法

以右圆环为例

（1）找到近端右拐点，且左边界不丢线

（2）找到右边中间拐点，定位近端右拐点，拉线

（3）右边界底部丢线，结束拉线，找远端右拐点

（4）定位远端右拐点，补线进环

（5）编码器计步大于某阈值，停止补线，进普通循迹状态

（6）赛道宽度突变，补线出环

（7）找远端右拐点，补线不入环

（8）结束，清空状态

void Right_Roll_Judge(void)
{
    extern int Distance_L;
    extern int Distance_R;

    int erro_x,erro_y;
    if(XK.roll_flag==0&&XK.roll_situation==0)//初始状态
    {
        if((Right_NearInflection_Judge(40,30)==1)&&(Left_Road_Lose(10,60)==1))//找到右拐点且左边界不丢失
        {
            XK.roll_situation=1;
            XK.roll_flag=1;
            XK.y3=28;
            erro_x=0;
            erro_y=0;
            XK.y2=0;
            XK.x2=0;
        }
    }
    if(XK.roll_situation==1)//找拐点，定位
    {
        Right_NearInflection_Catch(XK.y,60,XK.x,XK.y);//近拐点定位
        Right_RoundInflection_Catch(XK.y,1,XK.x2,XK.y2);//中间拐点定位
        if(XK.y>0&&XK.y2>0)//找到两个拐点，准备拉线
        {
            erro_x=0;
            erro_y=0;
            XK.y1=XK.y2;
            XK.x1=XK.x2;
            XK.roll_situation=2;
        }
    }
    if(XK.roll_situation==2)//拉线
    {

        Right_NearInflection_Catch((XK.y-2),60,XK.x2,XK.y2);
        erro_y=abs(XK.y2-20);

        XK.y3=XK.y;
        XK.y4=XK.y1+erro_y;
        XK.x3=XK.x;
        XK.x4=XK.x1;
        XK.line=1;
        if(XK.right_line[100]>160)//当右边界底部丢线，结束拉线
        {
            XK.line=0;
            XK.y2=0;
            XK.roll_situation=3;
        }
    }
    if(XK.roll_situation==3)//找远拐点
    {
        Right_FarInflection_Catch(1,40,XK.x2,XK.y2);//远拐点定位
        if((XK.y2>0)&&(XK.right_line[20]>150))//找到远拐点且右边界前端丢线，准备拉线
        {
            XK.y1=XK.y2;
            XK.x1=XK.x2;
            Distance_L=0;
            Distance_R=0;
            XK.Encoder_distance=0;
            XK.roll_situation=4;
        }
    }
    if(XK.roll_situation==4)//入环补线
    {

        XK.y3=25;
        XK.y4=55;
        XK.x3=110;
        XK.x4=45;
        XK.line=1;

        if(XK.Encoder_distance>5)//编码器计步（走一段距离，结束补线）
        {
            XK.y2=0;
            XK.line=0;
            Distance_L=0;
            Distance_R=0;
            XK.Encoder_distance=0;
            XK.roll_situation=6;
        }
    }
    if(XK.roll_situation==6)//环内状态
    {
        if((XK.road_width[29]-XK.road_width[30]>50)||XK.road_width[30]>150)//赛道宽度突变
        {
            XK.y1=XK.y2;
            XK.x1=XK.x2;
            XK.y2=0;
            Distance_L=0;
            Distance_R=0;
            XK.Encoder_distance=0;
            XK.roll_situation=7;
        }
    }
    if(XK.roll_situation==7)//出环补线
    {
        XK.y3=25;
        XK.y4=60;
        XK.x3=115;
        XK.x4=30;
        XK.line=1;
        Right_FarInflection_Catch(1,30,XK.x2,XK.y2);找远端拐点
        distence_count();
        if((XK.y2>0)&&XK.Encoder_distance>7)//不入环判断：在走一段距离的情况下，找到远端拐点
        {
            XK.y1=XK.y2;
            XK.x1=XK.x2;
            XK.roll_situation=8;
        }
    }
    if(XK.roll_situation==8)//不入环补线
    {
        XK.y3=100;
        XK.y4=20;
        XK.x3=136;
        XK.x4=113;
        XK.line=1;
    }
    if(XK.roll_situation==8)//不入环补线
    {
        Distance_L=0;
        Distance_R=0;
        XK.Encoder_distance=0;
        XK.y3=100;
        XK.y4=20;
        XK.x3=136;
        XK.x4=113;
        XK.line=1;
    }
    if((Left_Road_Lose(20,60)==1)&&XK.roll_situation==8)//右边界不丢失，圆环标志位清零
    {
        XK.roll_situation=0;
        XK.roll_flag=0;
        XK.line=0;
    }

}

void draw_line(void)//补线处理函数
{
    if(XK.line==1)
    {
         ips114_add_line(XK.y,XK.x+0,XK.y1,XK.x1+0,0x00);
        ips114_add_line(XK.y,XK.x+1,XK.y1,XK.x1+1,0x00);
        ips114_add_line(XK.y,XK.x+2,XK.y1,XK.x1+2,0x00);
         ips114_add_line(XK.y,XK.x+3,XK.y1,XK.x1+3,0x00);
        ips114_add_line(XK.y,XK.x-1,XK.y1,XK.x1-1,0x00);
        ips114_add_line(XK.y,XK.x-2,XK.y1,XK.x1-2,0x00);
    }
}

关于如何在图像数组中补线，基于逐飞的IPS114画线函数更改即可

void ips114_add_line (uint16 x_start, uint16 y_start, uint16 x_end, uint16 y_end, const uint16 color)
{

    int16 x_dir = (x_start<x_end ? 1:-1);
    int16 y_dir = (y_start<y_end ? 1:-1);
    float temp_rate = 0;
    float temp_b = 0;

    if(x_start != x_end)
    {
        temp_rate = (float)(y_start-y_end)/(float)(x_start-x_end);
        temp_b = (float)y_start-(float)x_start*temp_rate;
    }
    else
    {
        while(y_start != y_end)
        {
            mt9v03x_image_copy[x_start][ y_start]=0x00;//根据需要更改的图像数组

            y_start += y_dir;
        }
        return;
    }
    if(abs(y_start-y_end)>abs(x_start-x_end))
    {
        while(y_start != y_end)
        {
            mt9v03x_image_copy[x_start][ y_start]=0x00;//根据需要更改的图像数组
            y_start += y_dir;
            x_start = (int16)(((float)y_start-temp_b)/temp_rate);
        }
    }
    else
    {
        while(x_start != x_end)
        {
            mt9v03x_image_copy[x_start][ y_start]=0x00;//根据需要更改的图像数组
            x_start += x_dir;
            y_start = (int16)((float)x_start*temp_rate+temp_b);
        }
    }
}

2.电磁圆环，摄像头辅助判断

void elec_roll_judge(void)
{
// ********* 摄像头区分左右环（这一段可单独放在图像处理函数里） ***********
    if(XK.roll_flag==1&&XK.roll_situation==1)
    {
        XK.rolldirr=rroll_dir_judge(10,50,65);
        XK.rolldirl=lroll_dir_judge(10,50,65);
    }
// *****************************************************************
    extern float AD[6];
    if(XK.roll_flag==0&&XK.roll_situation==0)
    {
        if(AD[1]>3000&&AD[0]>3000)//中间电感大于一定阈值
        {
            XK.roll_flag=1;
            XK.roll_situation=1;
            Distance_L=0;
            Distance_R=0;
            XK.Encoder_distance=0;
        }
    }
        if(XK.roll_flag==1&&XK.roll_situation==1)
        {

            if(XK.rolldirr==1)//右圆环状态
            {
                XK.roll_flagr=1;
                XK.roll_flagl=0;
                XK.roll_situation=2;
                Distance_L=0;
                Distance_R=0;
                XK.Encoder_distance=0;
            }
            else if(XK.rolldirl==1)//左圆环状态
            {
                XK.roll_flagr=0;
                XK.roll_flagl=1;
                XK.roll_situation=2;
                Distance_L=0;
                Distance_R=0;
                XK.Encoder_distance=0;
            }
            if(XK.Encoder_distance>15)//计步大于某值，跳出圆环
             {
                XK.roll_situation=0;
                XK.roll_flag=0;
                XK.roll_flagl=0;
                XK.roll_flagr=0;
                Distance_L=0;
                Distance_R=0;
                XK.Encoder_distance=0;
             }
        }
        if(XK.roll_flag==1&&XK.roll_situation==2)
        {
            if(XK.roll_flagl==1&&XK.roll_flagr==0)//右圆环状态
            {
                if(AD[3]>3300)//右斜电感大于阈值，舵机打角
                {
                    XK.roll_situation=3;
                    Distance_L=0;
                    Distance_R=0;
                    XK.Encoder_distance=0;
                }
            }

            if(XK.roll_flagl==0&&XK.roll_flagr==1)//左圆环状态
            {
                if(AD[4]>3400)//左斜电感大于阈值，舵机打角
                {
                    XK.roll_situation=3;
                    Distance_L=0;
                    Distance_R=0;
                    XK.Encoder_distance=0;
                }
            }
        }
        if(XK.roll_flag==1&&XK.roll_situation==3)//环内状态
        {
            if(XK.Encoder_distance>9)//计步
            {
                XK.roll_situation=5;
                Distance_L=0;
                Distance_R=0;
                XK.Encoder_distance=0;
            }
        }
        if(XK.roll_flag==1&&XK.roll_situation==5)
        {
            if(XK.roll_flagl==1&&XK.roll_flagr==0)//右圆环出环判断
            {
                if(AD[5]>1700)//右侧电感大于某阈值，右打角
                {
                    XK.roll_situation=6;
                    Distance_L=0;
                    Distance_R=0;
                    XK.Encoder_distance=0;
                }
            }
            if(XK.roll_flagl==0&&XK.roll_flagr==1)//左圆环出环判断
            {
                if(AD[2]>2800)//左侧电感大于某阈值，左打角
                {
                    XK.roll_situation=6;
                    Distance_L=0;
                    Distance_R=0;
                    XK.Encoder_distance=0;
                }
            }
        }
   
        if(XK.roll_flag==1&&XK.roll_situation==6)//计步大于某阈值，结束圆环，标志位清零
        {
            if(XK.Encoder_distance>3)
            {
                XK.roll_situation=0;
                XK.roll_flag=0;
                XK.roll_flagr=0;
                XK.roll_flagl=0;
                Distance_L=0;
                Distance_R=0;
                XK.Encoder_distance=0;
            }
        }
}

二、避障

这里需要用到编码器计行驶圈数

#define ENCORDER_PRCISION   1024.f  //512线、1024线等
#define ENCORDER_D      19.2f   //编码器齿轮直径   齿数30
#define WHEEL_D         64.0f   //车轮直径
#define WHEEL_GEAR_D            41.0f   //车轮直连齿轮直径    齿数  68
#define GET_DISTANCE_M(val)    ((((val/ENCORDER_PRCISION)*ENCORDER_D*PI )*WHEEL_D)/WHEEL_GEAR_D/1000)

void distence_count(void)
{

    Distance_L = Distance_L + XK.speedl;
    Distance_R = Distance_R + XK.speedr;
    XK.Encoder_distance=GET_DISTANCE_M((Distance_L + Distance_R) / 2);

}

void barrier (void)
{
    extern int Distance_L;//左轮距
    extern int Distance_R;//右轮距

    if(dl1a_distance_mm<XK.dis1&&XK.barrier_judge==0)//避障预判断
    {
        Gyro_qin0();//陀螺仪数值清零
        XK.barrier_in=1;//避障状态
        XK.barrier_judge=1;//避障进入标志位
        XK.Encoder_distance=0;//测距距离

    }
    if(XK.barrier_judge==1)
    {
        switch (XK.barrier_in)
        {
        case 1:
        {
            if(dl1a_distance_mm<=XK.dis2)//行进距离
            {
                if((mt9v03x_image[2][94]+mt9v03x_image[3][94]+mt9v03x_image[3] 
                [93]+mt9v03x_image[3][95])/4<55)//tof测距小于某阈值且图像灰度像素小于某阈值                                                                        
                                                                        （区分坡道与避障）
                {
                Distance_L=0;
                Distance_R=0;
                XK.Encoder_distance=0;
                XK.barrier_in=2;
                Gyro_qin0();
                wireless_uart_send_string("6");//与后车通信
                }
                else//未达条件跳出状态
                {
                    Distance_L=0;
                    Distance_R=0;
                    XK.time=0;
                    XK.Encoder_distance=0;
                    XK.barrier_in=0;
                    XK.barrier_judge=0;
                    Gyro_qin0();
                }
            }
        }
        break;
        case 2://打角避障
        {

            if(XK.dash_dir==1)//右边避障
            {
              if(eulerAngle.yaw<-3.1)//右航向角
              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=3;
                Gyro_qin0();//陀螺仪数值清零
              }
            }
            if(XK.dash_dir==0)//左边避障
            {
              if(eulerAngle.yaw>1.6)//左航向角
              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=3;
                Gyro_qin0();//陀螺仪数值清零

              }
            }
        }
        break;
        case 3://直行
        {
            distence_count();//编码器测距
            if(XK.Encoder_distance>3)//行进距离
            {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=4;
                Gyro_qin0();
            }
        }
        break;
        case 4://打角
        {
           
            if(XK.dash_dir==1)//左边避障
            {
              if(eulerAngle.yaw>3.6)//左航向角
              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=5;
                Gyro_qin0();
              }
            }
            if(XK.dash_dir==0)//右边避障
            {
              if(eulerAngle.yaw<-6.0)//右航向角
              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=5;
                Gyro_qin0();
              }
            }
        }
        break;
        case 5://直行
        {
            distence_count();//编码器测距
            if(XK.Encoder_distance>1)//行进距离
            {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=6;
                Gyro_qin0();
            }
        }
        break;
        case 6://打角
        {
           
            if(XK.dash_dir==1)//左边避障
            {
              if(eulerAngle.yaw>5.0)//左航向角
              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=7;
                Gyro_qin0();
              }
            }
            if(XK.dash_dir==0)//右边避障
            {
              if(eulerAngle.yaw<-5.9)//右航向角
              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=7;
                Gyro_qin0();
              }
            }
        }
        break;
        case 7://直行，回赛道
        {
            extern float AD[6];
            distence_count();//编码器测距
            if(XK.dash_dir==1)
            {
              if((mt9v03x_image[110][140]+mt9v03x_image[110][141]+mt9v03x_image[110] 
                  [139])/3>100)//图像灰度像素大于某阈值（白色赛道）

              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=0;
                XK.barrier_judge=0;
               
              }
            }
            if(XK.dash_dir==0)
            {
              if((mt9v03x_image[110][48]+mt9v03x_image[110][49]+mt9v03x_image[110] 
              [50])/3>100)//图像灰度像素大于某阈值（白色赛道）

              {
                Distance_L=0;
                Distance_R=0;
                XK.time=0;
                XK.Encoder_distance=0;
                XK.barrier_in=0;
                XK.barrier_judge=0;
               
              }
            }
        }
        break; 
      }
    }
}

本文旨在本校智能车实验室教学，若有错漏，欢迎指正

标签：人工智能

本文转载自: https://blog.csdn.net/m0_62634962/article/details/132196956
版权归原作者 eTao_757 所有，如有侵权，请联系我们删除。

智能车竞赛圆环、避障等元素方案开源

一、圆环

1.基于传统扫线的补线方法

2.电磁圆环，摄像头辅助判断

二、避障

发表评论

“智能车竞赛圆环、避障等元素方案开源”的评论:

关于作者

overfit同步小助手

相关阅读

文章导航