#include "camera.h"
#include "zf_device_mt9v03x.h"
#include "math.h"
/******此处设置为偏差几行到几行,间距最好是40,方便调PID********/
#define CONTROLL_H_MAX 90
#define CONTROLL_H_MIN 50
/**********环岛处理相关结构体和变量**********/
#define ROUND_H_MAX 90 //定义环岛判断下限行
#define ROUND_H_MIN 60 //定义环岛判断上限行
#define LINE_NUM 15 //定义环岛左右边界差值宽度大于阈值行数
#define ROUNDABOUT_N 30 //定义判断环岛左右边界宽度阈值
#define ROUNDABOUT_K 0 //判断环岛边界斜率
#define ROUNDABOUT_E 5 //判断环岛边界的误差范围
/**********元素处理结构体**********/
st_Camera road_type, stage, distance, num;
uint8 image_yuanshi[MT9V03X_H][MT9V03X_W]; //原始图像
uint8 image_01[MT9V03X_H][MT9V03X_W]; //二值化图像
uint8 Threshold; //阈值
uint8 Threshold_static = 175; //阈值静态下限225
uint16 Threshold_detach = 255; //阳光算法分割阈值
uint8 search_line_end = 0; //搜线结束行
int16 l_line_x[MT9V03X_H], r_line_x[MT9V03X_H], m_line_x[MT9V03X_H]; //储存原始图像的左右边界的列数
uint8 l_lose_value = 0, r_lose_value = 0; //左右丢线数
uint8 l_search_flag[MT9V03X_H], r_search_flag[MT9V03X_H]; //是否搜到线的标志
uint8 l_width, r_width; //循环变量名
uint8 l_search_start, r_search_start; //搜线起始x坐标
uint8 l_line_x_yuanshi[MT9V03X_H], r_line_x_yuanshi[MT9V03X_H];
uint8 midcourt_line[MT9V03X_H]; //中线
uint8 Move_column = 20; //搜线移动x列,默认20列
////摄像头处理全流程
float left_slope=0,right_slope=0;//左边界和右边界的斜率
int16 left_error=0,right_error=0;//左边界和右边界的误差
int16 left_dis=0,right_dis=0;
void Camera_All_Deal(void)
{
if(mt9v03x_finish_flag == 1) //mt9v03x_finish_flag 是逐飞库里面场中断的标志位//图像采集完成(自用)
{
Transfer_Camera(mt9v03x_image[0], image_yuanshi[0], MT9V03X_W*MT9V03X_H); //图像转存
Get01change_Dajin(); //图像二值化
Search_Line(); //搜线
Element_Test(); //元素识别
//Show_Left_Cirque();
Element_Handle(); //元素处理
Integration();
Calculate_Offset(); //计算偏差
mt9v03x_finish_flag = 0; //在图像使用完毕后 务必清除标志位,否则不会开始采集下一幅图像
}
ips200_displayimage03x(image_01[0], MT9V03X_W, MT9V03X_H);//显示二值化黑白图像
//ips200_displayimage03x(image_yuanshi[0], MT9V03X_W, MT9V03X_H);//显示灰度图像,此部分我主要用于调试断路区域的
// Blacking();
}
void Camera_Parameter_Init(st_Camera *sptr)
{
sptr->Turnoff = 0;
sptr->straight = 0;
sptr->bend = 0;
}
//-----------------------------------------------------------------------------------------
// @brief 图像转存
// @param *p 待转存数组地址
// @param *q 转存后数组地址
// @param pixel_num 数组长度
// @return void
// Sample usage: Transfer_Camera(mt9v03x_image[0], image_yuanshi[0], MT9V03X_W*MT9V03X_H);
//-----------------------------------------------------------------------------------------
void Transfer_Camera(uint8 *p, uint8 *q, int16 pixel_num)
{
for(int16 i = 0; i < pixel_num; i++)
*(q +i) = *(p +i);
}
//-----------------------------------------------------------------------------------------
// @brief 求阈值并二值化
// @param void
// @return void
// Sample usage: Get01change_Dajin();
// explain 该函数里面调用了Threshold_Deal(image_yuanshi[0], MT9V03X_W, MT9V03X_H, Threshold_detach);
//-----------------------------------------------------------------------------------------
void Get01change_Dajin(void)
{
Threshold = Threshold_Deal(image_yuanshi[0], MT9V03X_W, MT9V03X_H, Threshold_detach);//最佳阈值
if (Threshold < Threshold_static)//最佳阈值的下限保护
{
Threshold = Threshold_static;
}
uint8 thre;
for(uint8 y = 0; y < MT9V03X_H; y++) //这里考虑到了图像边缘的光照偏弱的问题
{
for(uint8 x = 0; x < MT9V03X_W; x++)
{
if (x <= 15) //受影响的边缘范围
thre = Threshold - 10; //修改可调节图像边缘的清晰度
else if (x >= MT9V03X_W-15)
thre = Threshold - 10;//
else
thre = Threshold;
if (image_yuanshi[y][x] >thre) //数值越大,显示的内容越多,较浅的图像也能显示出来
image_01[y][x] = 255; //白
else
image_01[y][x] = 0; //黑
}
}
}
//-----------------------------------------------------------------------------------------
// @brief 求阈值
// @param *image 待求阈值的图像数组
// @param x 图像宽度
// @param y 图像高度
// @param pixel_threshold 阳光算法分割阈值
// @return void
// Sample usage: Threshold_Deal(image_yuanshi[0], MT9V03X_W, MT9V03X_H, Threshold_detach);
// explain: 该函数在Get01change_Dajin();里面被调用
// Threshold = Threshold_Deal(image_yuanshi[0], MT9V03X_W, MT9V03X_H, Threshold_detach);
//-----------------------------------------------------------------------------------------
uint8 Threshold_Deal(uint8* image, uint16 x, uint16 y, uint32 pixel_threshold)//最佳阈值计算函数
{
#define GrayScale 256
uint16 width = x; //总宽度
uint16 height = y; //总高度
int pixelCount[GrayScale]; //单个灰度值像素块的总个数存储数组
float pixelPro[GrayScale]; //用pixelcount除以pixelsum,得到单个灰度值在总图像中所占的比例
int i, j;
int pixelSum = width * height; //像素块的总面积
uint8 threshold = 0;
uint8* data = image; //指向像素数据的指针
for (i = 0; i < GrayScale; i++) //对图像进行初始化刷新
{
pixelCount[i] = 0;
pixelPro[i] = 0;
}
uint32 gray_sum = 0;
//统计灰度级中每个像素在整幅图像中的个数
for (i = 0; i < height; i += 1)
{
for (j = 0; j < width; j += 1)
{
// if((sun_mode&&data[i*width+j]<pixel_threshold)||(!sun_mode))
//{
pixelCount[(int)data[i * width + j]]++; //将当前的点的像素值作为计数数组的下标
gray_sum += (int)data[i * width + j]; //灰度值总和
//}
}
}
//计算每个像素值的点在整幅图像中的比例
for (i = 0; i < GrayScale; i++)
{
pixelPro[i] = (float)pixelCount[i] / pixelSum;
}
//遍历灰度级[0,255]
float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;
w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
for (j = 0; j < pixel_threshold; j++)
{
w0 +=pixelPro[j]; //背景部分每个灰度值的像素点所占比例之和 即背景部分的比例
u0tmp += j * pixelPro[j]; //背景部分 每个灰度值的点的比例 *灰度值
w1 = 1 - w0; //前景部分每个灰度值的像素点所占比例之和 即前景部分的比例
u1tmp = gray_sum / pixelSum - u0tmp; //前景部分 每个灰度值的点的比例 *灰度值
/*
等价于:
w1+=pixelPro[j];
u1tmp+=j*pixelPro[j];
*/
u0 = u0tmp / w0; //背景平均灰度
u1 = u1tmp / w1; //前景平均灰度
u = u0tmp + u1tmp; //全局平均灰度
deltaTmp = w0 * pow((u0 - u), 2) + w1 * pow((u1 - u), 2);
if (deltaTmp > deltaMax)
{
deltaMax = deltaTmp;
threshold = (uint8)j;
}
if (deltaTmp < deltaMax)
{
break;
}
}
return threshold;
}
//-----------------------------------------------------------------------------------------
// @brief 搜左右边线
// @param void
// @return void
// Sample usage: Search_Line();
//-----------------------------------------------------------------------------------------
void Search_Line(void)
{
r_lose_value = l_lose_value = 0; //搜线之前将丢线数清零
for(uint8 y = MT9V03X_H - 1; y > search_line_end; y--) //确定每行的搜线起始横坐标//起始条件:初始行是最高行的下面一行 循环条件:结束行大于搜线结束行
{
if( y > MT9V03X_H - 2) //前5行从两边往中间搜线
{
l_search_start = 5; //3
r_search_start = 183;//185
}
else
{
// l_search_start = midcourt_line[y+1] - Move_column ; //后边行左边从上一行中线左移X列搜线
// r_search_start= midcourt_line[y+1] + Move_column; //后边行右边从上一行中线右移X列搜线
if(l_search_flag[y+1]&&r_search_flag[y+1])
{
l_search_start=l_line_x[y+1]-Move_column; //后边行左边从上一行中线左移X列搜线
r_search_start=r_line_x[y+1]+Move_column; //后边行右边从上一行中线左移X列搜线
if(l_search_start<5) l_search_start=5; //防止搜线起始行溢出
if(r_search_start>(MT9V03X_W-5)) r_search_start=MT9V03X_W-5;
}
else
{
l_search_start=5;
r_search_start=183;
}
}
for(l_width = l_search_start; l_width < MT9V03X_W ; l_width++) //左边搜线
{
if(image_01[y][l_width -2] == 0 && image_01[y][l_width - 1] != 0 && image_01[y][l_width] != 0 )
{//黑白白
l_line_x[y] = l_width - 1;
l_line_x_yuanshi[y] = l_width - 1;
l_search_flag[y] = 1;
break;
}
else if(l_width == MT9V03X_W-3) //搜到最右边没有搜到判定丢线
{
l_line_x[y] = MT9V03X_W/2;
l_line_x_yuanshi[y] = MT9V03X_W/2;
l_search_flag[y] = 0;
l_lose_value++;
break;
}
}
for(r_width = r_search_start; r_width > 0; r_width--) //右边搜线
{
if(image_01[y][r_width] != 0 && image_01[y][r_width +1] != 0 && image_01[y][r_width +2] == 0 && r_width > MT9V03X_W/2 )
{//白白黑
r_line_x[y] = r_width + 1;
r_line_x_yuanshi[y] = r_width + 1;
r_search_flag[y] = 1;
break;
}
else if(r_width == 3) //搜到最左边没有搜到判定丢线
{
r_line_x[y] = MT9V03X_W/2;
r_line_x_yuanshi[y] = MT9V03X_W/2;
r_search_flag[y] = 0;
r_lose_value++;
break;
}
}
midcourt_line[y] = (r_line_x[y] + l_line_x[y]) / 2; //计算中线
if(l_search_flag[y]==0&&r_search_flag[y]==0&&(y!=MT9V03X_H-1))
{
midcourt_line[y]=midcourt_line[y+1];
}
}
}
//-----------------------------------------------------------------------------------------
// @brief 偏差计算
// @param void
// @return void
// Sample usage: Calculate_Offset();
//-----------------------------------------------------------------------------------------
int32 offset_1;
float offset; //摄像头处理得到的偏差(车在偏左侧是负值,在偏右侧是正值)
void Calculate_Offset(void)
{
offset_1 = 0;
for(uint8 y = CONTROLL_H_MAX; y >=CONTROLL_H_MIN; y=y-2)
{
offset_1 += (midcourt_line[y] - MT9V03X_W/2+5);
}
offset = (float)(offset_1/20);
if (road_type.LeftAngle) //左直角处理
{
offset = 75; //-10
}
else if (road_type.RightAngle)//右直角处理
{
offset = -75; //10
}
if(road_type.Turnoff) //断路处理
{
offset = 0; //6
}
}
//-----------------------------------------------------------------------------------------
// @brief 画边线和中线(彩色)
// @param void
// @return void
// Sample usage: Blacking();
// explain: 如果屏幕显示的图像进行了缩放就不能适用
//-----------------------------------------------------------------------------------------
void Blacking()
{
for(uint8 y=(MT9V03X_H-1); y>search_line_end; y--)
{
ips200_draw_point(l_line_x[y], y, RGB565_GREEN);
ips200_draw_point(r_line_x[y], y, RGB565_YELLOW);
ips200_draw_point( midcourt_line[y], y, RGB565_BLUE);
}
for(uint8 i =0; i<MT9V03X_W; i++)
{
ips200_draw_point(i, CONTROLL_H_MAX, RGB565_BLUE);
ips200_draw_point(i, CONTROLL_H_MIN, RGB565_GREEN);
//ips200_draw_point(i, 20, RGB565_RED);
}
for(uint8 i=0; i<MT9V03X_H; i++)
{
ips200_draw_point(MT9V03X_W/2, i, RGB565_RED);
}
}
/**************************************************
* @brief 计算两点间斜率
* @param x1 第一个点的x坐标
* @param y1 第一个点的y坐标
* @param x2 第二个点的x坐标
* @param y2 第二个点的y坐标
* @return 斜率
**************************************************/
float Calculate_Slope_Two_Points(int16 x1, int16 y1, int16 x2, int16 y2)
{
if (x2 - x1 == 0) // 避免除以零
return 0.0f;
return (float)(y2 - y1) / (x2 - x1);
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 元素识别处理
// @param void
// @return void
// Sample usage: Element_Test();
//-------------------------------------------------------------------------------------------------------------------
int16 Car_gyro = 0;
int16 Car_speed = 0;
void Element_Test()
{
imu660ra_get_gyro(); //获取660陀螺仪角速度
Car_gyro= imu660ra_gyro_transition(imu660ra_gyro_z); // 单位为°/s
//speed_measure(); //编码器测量
Car_speed=(left_speed+right_speed)/2;
//计算左边界的斜率
left_slope=Calculate_Slope_Two_Points(l_line_x[ROUND_H_MAX],ROUND_H_MAX,l_line_x[ROUND_H_MIN],ROUND_H_MIN);
//计算右边界的斜率
right_slope=Calculate_Slope_Two_Points(r_line_x[ROUND_H_MAX],ROUND_H_MAX,r_line_x[ROUND_H_MIN],ROUND_H_MIN);
//计算左边界和右边界的误差
left_error=abs(l_line_x[ROUND_H_MAX]-l_line_x[ROUND_H_MIN]);
right_error=abs(r_line_x[ROUND_H_MAX]-r_line_x[ROUND_H_MIN]);
left_dis=l_line_x[ROUND_H_MAX]-l_line_x[ROUND_H_MIN];
right_dis=r_line_x[ROUND_H_MAX]-r_line_x[ROUND_H_MIN];
if(!road_type.LeftCirque && !road_type.RightCirque && !road_type.Turnoff&& !road_type.LeftAngle && !road_type.RightAngle)
{
Turnoff_Test(); //断路识别
Angle_Test(); //直角识别
Roundabout_Test(); //环岛识别
}
}
/********************用于积分****************/
uint32 Left_CirqueIntegration = 0, Right_CirqueIntegration = 0;
uint32 Left_AngleIntegration = 0, Right_AngleIntegration = 0;
uint32 TurnoffIntegration = 0;
uint32 Left_SpeedIntergration=0,Right_SpeedIntergration=0;
void Integration(void)
{
if(road_type.LeftCirque == 1)
{
Left_CirqueIntegration += fabs(Car_gyro)*0.1;
}
if(road_type.RightCirque == 1)
{
Right_CirqueIntegration += fabs(Car_gyro)*0.1;
}
if(road_type.LeftAngle == 1)
{
Left_AngleIntegration += fabs(Car_gyro)*0.1;
}
if(road_type.RightAngle == 1)
{
Right_AngleIntegration += fabs(Car_gyro)*0.1;
}
if(road_type.Turnoff == 1)
{
TurnoffIntegration += fabs(Car_gyro)*0.1;
}
}
void Element_Handle()
{
if(road_type.LeftCirque)
{
LeftCirque_Handle(); //左环岛处理
}
else if (road_type.RightCirque)
{
RightCirque_Handle(); //右环岛处理
}
if (road_type.LeftAngle) //左直角处理
{
LeftAngle_Handle();
}
else if (road_type.RightAngle)//右直角处理
{
RightAngle_Handle();
}
if(road_type.Turnoff)
{
Turnoff_Handle(); //断路处理
}
}
#define BREAK_THRESHOLD 80 //断路丢线阈值
int16 lost_count = 0; //连续丢线数
void Turnoff_Test()
{
uint8 consecutive = 0; //左右连续丢线数
for(uint8 y = MT9V03X_H-1; y >10; y--)
{
if(l_search_flag[y]==0 && r_search_flag[y]==0) //左右丢线标志位都为0
{
consecutive++;
if(consecutive>BREAK_THRESHOLD) //左右连续丢线数大于阈值(BREAK_THRESHOLD)时判定为断路
{
road_type.Turnoff = 1;
//lost_count= consecutive;
break;
}
}
else
{
consecutive = 0;
}
}
}
#define ANGLE_H_MAX 90 //定义直角判断下限行
#define ANGLE_H_MIN 50 //定义直角判断上限行
#define ANGLE_H_THRESHOLD 5 //上下各20行丢线和不丢线阈值数10
#define ANGLE_X_HAVE 30 //平移扫线列数20
uint16 L_H_Dif=0;
uint16 R_H_Dif=0;
int16 Road_error=0;
//直角识别
void Angle_Test(void)
{
uint8 have_wired = 0; //下20行不丢线标志位
uint8 none_wired = 0; //上20行丢线标志位
uint8 consecutiv_have = 0; //左右不丢线总数
uint8 consecutive_none = 0; //左右丢线总数
uint8 L_num=0,R_num=0;
uint8 L_flag=0;
uint8 R_flag=0;
for(uint8 y = ANGLE_H_MAX-1; y >= ANGLE_H_MIN; y--)
{
if(y>(ANGLE_H_MAX+ANGLE_H_MIN)/2) //下20行不丢线判断
{
if(l_search_flag[y]==1 && r_search_flag[y]==1&&abs(midcourt_line[y]-MT9V03X_W/2)<20) //左右不丢线标志位都为1
{
consecutiv_have++;
if(consecutiv_have>ANGLE_H_THRESHOLD+5) //左右不丢线数大于阈值
{
have_wired = 1; //下20行至少有10行不丢线
}
}
}
else //上20行丢线判断
{
if(l_search_flag[y]==0 && r_search_flag[y]==0)//左右不丢线标志位都为0
{
consecutive_none++;
if(consecutive_none>ANGLE_H_THRESHOLD) //左右丢线数大于阈值
{
none_wired = 1; //上20行至少有10行丢线
}
}
}
}
//取40行,下20行至少有10行不丢线,上20行至少有10丢线,进行直角下一步判断
//判断左直角:从中线MT9V03X_W/2往左各平移20,40,60行从下往上扫线,三列有一列扫到白点则认为是左直角
//判断右直角:从中线MT9V03X_W/2往右各平移20,40,60行从下往上扫线,三列有一列扫到白点则认为是右直角
if( have_wired && none_wired ) //直角下一步判断
{
//左边扫线
for(uint8 wight= MT9V03X_W/2; wight > 5 ; wight--)
{
for(uint8 high=ANGLE_H_MAX;high>ANGLE_H_MIN;high--)//从下往上搜线
{
//左边三列扫线
if(image_01[high][wight] == 0 && image_01[high-1][wight] != 0 && image_01[high-2][wight] != 0 )
{//黑白白
L_num++;
break;
}
}
if(L_num>ANGLE_X_HAVE)
{
L_flag=1;
}
}
//右边扫线
for(uint8 wight= MT9V03X_W/2; wight <MT9V03X_W-5 ; wight++)
{
for(uint8 high=ANGLE_H_MAX;high>ANGLE_H_MIN;high--)//从下往上搜线
{
if(image_01[high][wight] == 0 && image_01[high-1][wight] != 0 && image_01[high-2][wight] != 0)
{//黑白白
R_num++;
break;
}
}
if(R_num>ANGLE_X_HAVE)
{
R_flag=1;
}
}
}
if(L_flag==1)
{
road_type.LeftAngle=1;
have_wired=0;
none_wired=0;
L_num=0;
L_flag=0;
}
if(R_flag==1)
{
road_type.RightAngle=1;
have_wired=0;
none_wired=0;
R_num=0;
R_flag=0;
}
if(L_flag==1&&R_flag==1)
{
road_type.LeftAngle=0;
road_type.RightAngle=0;
}
}
/**************************************************
* @brief 环岛识别
* @param void
* @return void
**************************************************/
void Roundabout_Test(void)
{
uint8 Roundabout_Maby=0;
uint16 width_count=0;//宽度计数
for(uint8 y=ROUND_H_MAX-1;y>ROUND_H_MIN;y--)
{
//判断是否可能是环岛
if(abs(r_line_x[y]-l_line_x[y])>ROUNDABOUT_N&&r_search_flag[y]&&l_search_flag[y]&&(right_error<ROUNDABOUT_E||left_error<ROUNDABOUT_E))
{
width_count++;
if(width_count>LINE_NUM)
{
Roundabout_Maby=1;
}
}
}
if(Roundabout_Maby==1&&left_slope<-0.8&&right_error<ROUNDABOUT_E&&left_dis<-30)
{
road_type.LeftCirque=1;
Roundabout_Maby=0;
BUZZ_ON;
}
if(Roundabout_Maby==1&&right_slope>0.8&&left_error<ROUNDABOUT_E&&left_dis>30)
{
road_type.RightCirque=1;
Roundabout_Maby=0;
BUZZ_ON;
}
//ips200_show_uint(0,240,width_count,5);
}
/**************************************************
* @brief 左环岛处理
* @param void
* @return void
**************************************************/
void LeftCirque_Handle(void)
{
if(road_type.LeftCirque==1)
{
switch(stage.LeftCirque)
{
case 0:
if(left_slope>2.0&&left_dis>10&&right_error<ROUNDABOUT_E)
{
stage.LeftCirque=1;
BUZZ_ON;
}
break;
case 1:
if(Left_CirqueIntegration>250)
{
stage.LeftCirque=2;
BUZZ_OFF;
}
break;
case 2:
if(Left_CirqueIntegration>850)
{
stage.LeftCirque=3;
BUZZ_ON;
}
break;
case 3:
if(Left_CirqueIntegration>350)
{
stage.LeftCirque=4;
}
break;
}
if(stage.LeftCirque==0)
{
//布线
for(uint8 i = CONTROLL_H_MAX; i >CONTROLL_H_MIN; i--)
{
midcourt_line[i]=r_line_x[i];
}
}
if (stage.LeftCirque==1)
{
//中线 = 左边界(进入环岛)
for(uint8 i = CONTROLL_H_MAX; i >CONTROLL_H_MIN; i--)
{
midcourt_line[i]=l_line_x[i];
}
}
if(stage.LeftCirque==2)
{
}
if (stage.LeftCirque==3)
{
//中线 = 右边界(出环岛)
for(uint8 i = CONTROLL_H_MAX-1; i >CONTROLL_H_MIN; i--)
{
midcourt_line[i]=r_line_x[i];
}
}
if(stage.LeftCirque==4)
{
Left_CirqueIntegration=0;
Left_SpeedIntergration=0;
stage.LeftCirque=0;
road_type.LeftCirque=0;
BUZZ_OFF;
}
}
}
/**************************************************
* @brief 右环岛处理
* @param void
* @return void
**************************************************/
void RightCirque_Handle(void)
{
if(road_type.RightCirque==1)
{
switch(stage.RightCirque)
{
case 0:
if(left_slope<-2.0&&left_dis<-10&&right_error<ROUNDABOUT_E)
{
stage.RightCirque=1;
BUZZ_ON;
}
break;
case 1:
if(Right_CirqueIntegration>250)
{
stage.RightCirque=2;
BUZZ_OFF;
}
break;
case 2:
if(Right_CirqueIntegration>850)
{
stage.RightCirque=3;
BUZZ_ON;
}
break;
case 3:
if(Right_CirqueIntegration>350)
{
stage.RightCirque=4;
}
break;
}
if(stage.RightCirque==0)
{
//布线
for(uint8 i = CONTROLL_H_MAX; i >CONTROLL_H_MIN; i--)
{
midcourt_line[i]=r_line_x[i];
}
}
if (stage.RightCirque==1)
{
//中线 = 左边界(进入环岛)
for(uint8 i = CONTROLL_H_MAX; i >CONTROLL_H_MIN; i--)
{
midcourt_line[i]=l_line_x[i];
}
}
if(stage.RightCirque==2)
{
}
if (stage.RightCirque==3)
{
//中线 = 右边界(出环岛)
for(uint8 i = CONTROLL_H_MAX-1; i >CONTROLL_H_MIN; i--)
{
midcourt_line[i]=r_line_x[i];
}
}
if(stage.RightCirque==4)
{
Right_CirqueIntegration=0;
Right_SpeedIntergration=0;
stage.RightCirque=0;
road_type.RightCirque=0;
BUZZ_OFF;
}
}
}
/**************************************************
* @brief 左直角处理
* @param void
* @return void
**************************************************/
void LeftAngle_Handle(void)
{
if(road_type.LeftAngle==1)
{
midcourt_line[CONTROLL_H_MAX-1]=MT9V03X_W/2;
for(uint8 y=CONTROLL_H_MAX-1;y>=CONTROLL_H_MIN;y--)
{
midcourt_line[y]-=100-y;
if(midcourt_line[y]<5) break;
}
if(Right_AngleIntegration>180)
{
road_type.LeftAngle=0;
Left_AngleIntegration=0;
BUZZ_OFF;
}
}
}
/**************************************************
* @brief 右直角处理
* @param void
* @return void
**************************************************/
void RightAngle_Handle(void)
{
if(road_type.RightAngle==1)
{
midcourt_line[CONTROLL_H_MAX-1]=MT9V03X_W/2;
for(uint8 y=CONTROLL_H_MAX-1;y>=CONTROLL_H_MIN;y--)
{
midcourt_line[y]+=100-y;
if(midcourt_line[y]>183) break;
}
if(Right_AngleIntegration>180)
{
road_type.RightAngle=0;
Right_AngleIntegration=0;
BUZZ_OFF;
}
}
}
/**************************************************
* @brief 断路处理
* @param void
* @return void
**************************************************/
void Turnoff_Handle(void)
{
if(TurnoffIntegration>220)
{
road_type.Turnoff = 0;
TurnoffIntegration=0;
BUZZ_OFF;
}
}
看直角识别与处理方面有没有什么问题
最新发布