摄像头组很稳定的黑线提取算法.docx
《摄像头组很稳定的黑线提取算法.docx》由会员分享，可在线阅读，更多相关《摄像头组很稳定的黑线提取算法.docx（33页珍藏版）》请在冰豆网上搜索。
摄像头组很稳定的黑线提取算法
//本程序对黑线提取做了很大的改进,未对十字弯处理
/*******************************************/
//本程序中加入了一些用于显示车模状态的LED灯
/*经实测发现，近处的赛道宽度为60左右，远处为30左右，所以用可变赛道宽度进行搜索*/
#include/*commondefinesandmacros*/
#include"derivative.h"/*derivative-specificdefinitions*/个人收集整理勿做商业用途
#include
//-----------函数声明-----------------//
voiddelay（unsignedintt）;
#defineHighSpeedLimit40
#defineLowSpeedLimit16
#defineSteerLeftLimit-35
#defineSteerRightLimit35
#defineCOLUMN90//采集列数
#defineMID_COLUMN45//中间黑线
#defineROW40//采集行数
#defineLeftLEDPORTB_PB0//左转方向灯
#defineRightLEDPORTB_PB1//右转方向灯
#defineSpeedUpLEDPORTB_PB2//加速指示灯
#defineSlowDownLEDPORTB_PB3//减速方向灯
#defineCrossing_RoadLEDPORTB_PB4//十字弯
#defineDashed_RoadLEDPORTB_PB5//虚线路段
#defineMid_Route_Width_Factor0.48//赛道宽度系数
intsteer_dire_label=0;
intSteerDelta=0;//舵机最终的偏转增量
unsignedintSpeed=0;//显示当前PWM2占空比大小
unsignedintPreSpeed=0;
floatThreshold_Factor=0.9;//阈值系数设置
unsignedintThreshold=120;//初设动态阈值为90,以后每传来一帧数据更新一次个人收集整理勿做商业用途
floatKp=0.8;//舵机方向比例系数
floatKd=5.0;//舵机方向微分系数
floatMotorSpeed_Factor=6.0;//马达控制
unsignedcharImage_Data[ROW][COLUMN];
unsignedintLeft[ROW],Right[ROW];//左右黑线
unsignedintVisualMiddle[ROW];//虚拟中线
unsignedintMiddle[ROW];//最终存放中间黑线值的二维数组
unsignedintRow_Attribute[ROW];//行属性
unsignedintrow,column;
intm=0;//计算采集到的行数
unsignedcharLine_Flag=0;//奇偶场
unsignedintLine_C=0;//采集行数dintPreSteerDirection=50;//之前的舵机方向,用与前后比较个人收集整理勿做商业用途
unsignedintLeftFlag=0,RightFlag=0;//左右黑线标志
unsignedintLeft_Start_Flag=0,Right_Start_Flag=0;//左右起始黑线找到标志个人收集整理勿做商业用途
unsignedintL_lost_count=0;//左黑线丢失计数
unsignedintR_lost_count=0;//右黑线丢失计数
unsignedintL_last_lost=0;//左行上一行丢线标志
unsignedintR_last_lost=0;//右行上一行丢线标志
unsignedintL_last_memory=0;//左行上一次有黑线的黑线所在列数
unsignedintR_last_memory=0;//右行上一次有黑线的黑线所在列数
unsignedintHS_Data[ROW]={40,45,50,55,60,65,70,75,80,85,个人收集整理勿做商业用途
90,95,100,105,110,115,120,124,128,132,
136,140,144,148,152,156,160,163,166,169,
172,175,178,181,184,187,189,191,193,194};
unsignedintHS_Pointer=0;//指向行数数组中的数据
interror[2]={0,0};//舵机PD调节时的误差参数
voiddelay（unsignedintTime）
{//一般,Time设为10000,可以实现1秒一次
inti,j;
for（i=0;ifor（j=0;j<8;j++）{
}
}
voidSlowDown（inttimer）{
inti;
PWMDTY2=0;
PWMDTY3=50;
for（i=0;i_asm（nop）;
PWMDTY3=0;
PWMDTY2=Speed;
}
/**************************************************个人收集整理勿做商业用途
**函数名称:
图像灰度值采集
**功能描述:
采集像素值
**输入:
无
**输出:
无
**说明：
***************************************************/个人收集整理勿做商业用途
voidData_collect（void）
{
intia=0;
while（ia{//这样把赛道取的窄一点
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
_asm（nop）;_asm（nop）;
Image_Data[Line_C][ia++]=PORTA;//portA是8位的,可表示0～255,即灰度值。
注意采集的数据先放在数组的后部,即从124->个人收集整理勿做商业用途
}
}
/***************************************************个人收集整理勿做商业用途
**函数名称:
PLL_Init
**功能描述:
时钟初始化函数
**说明:
BUSCLOCK=80M
****************************************************/个人收集整理勿做商业用途
voidPLL_Init（void）//pllclock=2*osc*（1+SYNR）/（1+REFDV）;个人收集整理勿做商业用途
{
CLKSEL=0x00;//disengagePLLtosystem
PLLCTL_PLLON=1;//turnonPLL
SYNR=0xc0|0x09;
REFDV=0x80|0x01;
POSTDIV=0x00;//pllclock=2*osc*（1+SYNR）/（1+REFDV）=160MHz;个人收集整理勿做商业用途
while（!
（CRGFLG_LOCK==1））;//whenpllissteady,thenuseit;个人收集整理勿做商业用途
CLKSEL_PLLSEL=1;//engagePLLtosystem;
}
/***********************************************/
/*P0输出频率为300Hz的方波,用于控制舵机*/
/*P2,P3输出频率为20KHZ,用于驱动电机的转速*/
/************************************************/
voidPWM_Init（void）
{
PWME_PWME0=0x00;//禁止P0
PWME_PWME2=0x00;//禁止P2
PWME_PWME3=0x00;//禁止P3
PWMPOL_PPOL0=1;//PWMPolarity0开始输出高电平.
PWMPOL_PPOL2=1;//PWMPolarity2开始输出高电平.
PWMPOL_PPOL3=1;//PWMPolarity3开始输出高电平.
PWMPRCLK=0x43;//01000011A=80M/8=10M,B=80M/16=5M时钟预分频个人收集整理勿做商业用途
PWMSCLA=150;//SA=A/（2*150）=33.33KHZ
PWMSCLB=20;//SB=B/（2*20）=125KHZ;
PWMCLK_PCLK0=1;//P0选的是SA时钟
PWMPOL_PPOL0=1;//选用开始为高电平方式
PWMCAE_CAE0=0;//选用左对齐方式
PWMCLK_PCLK2=1;//P2选的是SB时钟
PWMPOL_PPOL2=1;//选用开始为高电平方式
PWMCAE_CAE2=0;//选用左对齐方式
PWMCLK_PCLK3=1;//P2选的是SB时钟
PWMPOL_PPOL3=1;//选用开始为高电平方式
PWMCAE_CAE3=0;//选用左对齐方式
PWMCTL=0x00;//控制寄存器设置为无联接
//对舵机,驱动电机PWM波初始化
PWMDTY0=50;//P0占空比为50%
PWMDTY2=20;//P2占空比为50%
PWMDTY3=33;//P3占空比为50%
PWMPER0=100;//P0:
Frequency=SA/100=333.33hz
PWMPER2=100;//P2:
Frequency=SB/100=1.25KHZ
PWMPER3=100;//P3:
Frequency=SB/100=1.25KHZ
PWME_PWME0=1;//打开P0
PWME_PWME2=1;//打开P2
PWME_PWME3=1;//打开P3
}
/***************************************************个人收集整理勿做商业用途
**函数名称:
TIM_Init
**功能描述:
行场中断初始化函数
**说明:
****************************************************/个人收集整理勿做商业用途
voidTIM_Init（void）
{
TIOS=0x00;//定时器通道0,1为输入捕捉
TCTL4=0x09;//通道0捕捉上升沿,通道1捕捉下降沿
TIE=0x03;//通道0,1中断使能
TFLG1=0xFF;//清中断标志位
TSCR1=0x80;//定时器使能
}
/***************************************************个人收集整理勿做商业用途
**函数名称:
SCI0_Init
**功能描述:
串口1初始化函数
**说明:
PS2--RX,PS3--TX
****************************************************/个人收集整理勿做商业用途
voidSCI0_Init（）
{
SCI0BDL=（byte）（（80000000/*OSCfreq*//1）/57600/*baudrate*//16/*factor*/）;个人收集整理勿做商业用途
SCI0CR1=0X00;/*normal,noparity*/个人收集整理勿做商业用途
SCI0CR2=0X0C;/*RIE=1,TE=1,RE=1,*/个人收集整理勿做商业用途
}
/**************************************************个人收集整理勿做商业用途
**函数名称:
串口发射端程序
**功能描述:
发送赛道信息1为黑线0为白板
**输入:
无
**输出:
无
**说明：
***************************************************/个人收集整理勿做商业用途
voidSCI0_Transmit（unsignedchardata）
{
while（!
（SCI0SR1&0x80））;//如果SCI状态寄存器SCI1SR1的TDRE位为0,则一直执行这个无语句的循环,否则,执行下面语句个人收集整理勿做商业用途
SCI0DRL=data;//把要发送的数据存在SCI数据寄存器的低8位中
}
/***************************************************个人收集整理勿做商业用途
**函数名称:
IO_Init
**功能描述:
初始化函数
**说明:
****************************************************/个人收集整理勿做商业用途
voidIO_Init（void）
{
DDRA=0X00;//A端口的数据传送方向为输入
DDRB=0XFF;//端口B为输出
PORTB=0XFF;//端口B全部打开
}
/************************************************/
/*提取出图像的基本特征*/
/************************************************/
voidImage_Operates（）
{
chari,j;
charScan_Start,Scan_End;//扫描起始点，扫描终止点
charGate,Range;
unsignedcharTemp_Mid_Route;
intt,temp0;
Left_Start_Flag=0,Right_Start_Flag=0;//左右起始黑线找到标志
L_lost_count=0;//左黑线丢失计数
R_lost_count=0;//右黑线丢失计数
L_last_lost=0;//左行上一行丢线标志
R_last_lost=0;//右行上一行丢线标志
L_last_memory=0;//左行上一次有黑线的行计数器
R_last_memory=0;//右行上一次有黑线的行计数器
for（i=ROW-1;i>=0;i--）//数据初始化
{
Left[i]=0;
Right[i]=COLUMN;
VisualMiddle[i]=0;
Row_Attribute[i]=0;
}
for（i=ROW-1;i>=0;i--）
{
if（i>30）
{
Gate=45;
Range=40;
}
elseif（i>20）
{
Gate=35;
Range=35;
}
elseif（i>10）
{
Gate=25;
Range=30;
}
else
{
Gate=20;
Range=20;
}
LeftFlag=RightFlag=0;//左右黑线标志位清零
if（i>=ROW-2）//前两行全局扫描
{
//////////////////左边黑线起始行提取
for（j=MID_COLUMN;j>1;j--）
{
if（Image_Data[i][j]{
//if（（Image_Data[i][j+1]-Image_Data[i][j]>Gate）||（Image_Data[i][j+2]-Image_Data[i][j]>Gate））个人收集整理勿做商业用途
{
Left[i]=j;
Left_Start_Flag=1;//识别到黑线
LeftFlag=1;
break;
}
}
elseif（j==2）
{
Left[i]=0;
break;
}
}//endfor（j=MID_COLUMN;j>1;j--）
///////////////////////////////右边黑线起始行提取
for（j=MID_COLUMN;j{
if（Image_Data[i][j]{
//if（（Image_Data[i][j-1]-Image_Data[i][j]>Gate）||（Image_Data[i][j-2]-Image_Data[i][j]>Gate））个人收集整理勿做商业用途
{
Right[i]=j;
Right_Start_Flag=1;//识别到黑线
RightFlag=1;
break;
}
}
elseif（j==COLUMN-2）
{
Right[i]=COLUMN;
break;
}
}//endfor（m=MID_COLUMN;m}//endif（i<2）//前两行全局扫描
else//（i{
///////////////////左边
if（Left_Start_Flag&&!
L_last_lost）//识别到起始行且前一行没有丢失个人收集整理勿做商业用途
{
Scan_End=Left[i+1]-Range;
if（Scan_End<1）
Scan_End=1;
Scan_Start=Left[i+1]+Range;
if（Scan_Start>VisualMiddle[i+1]）
Scan_Start=VisualMiddle[i+1];
for（j=Scan_Start;j>Scan_End;j--）
if（Image_Data[i][j]{
//if（（Image_Data[i][j+1]-Image_Data[i][j]>Gate）||（Image_Data[i][j+2]-Image_Data[i][j]>Gate））个人收集整理勿做商业用途
{
Left[i]=j;
LeftFlag=1;//识别到黑线
break;
}
}
elseif（j==（Scan_End+1））
{
Left[i]=0;
L_last_lost=1;//这一行数据丢失标识位
L_last_memory=Left[i+1];
break;
}
}//endif（Left_Start_Flag&&!
L_last_lost）//识别到起始行且前一行没有丢失个人收集整理勿做商业用途
elseif（!
Left_Start_Flag）//没有识别到起始行，继续寻找起始行
{
for（j=VisualMiddle[i+1];j>1;j--）
{
if（Image_Data[i][j]{
//if（（Image_Data[i][j+1]-Image_Data[i][j]>Gate）||（Image_Data[i][j+2]-Image_Data[i][j]>Gate））个人收集整理勿做商业用途
{
Left[i]=j;
Left_Start_Flag=1;//识别到黑线
LeftFlag=1;
break;
}
}
elseif（j==2）
{
Left[i]=0;
break;
}
}//endfor（j=MID_COLUMN;j>1;j--）
}//endelseif（!
Left_Start_Flag）//没有主识别到起始行，继续寻找起始行个人收集整理勿做商业用途
elseif（L_last_lost）//识别到起始行但前一行丢失从虚拟中线开始循线
{
Scan_End=L_last_memory-Range;
if（Scan_End<1）
Scan_End=1;
for（j=VisualMiddle[i+1];j>Scan_End;j--）
{
if（Image_Data[i][j]{
//if（（Image_Data[i][j+1]-Image_Data[i][j]>Gate）||（Image_Data[i][j+2]-Image_Data[i][j]>Gate））个人收集整理勿做商业用途
{
Left[i]=j;
LeftFlag=1;
L_last_lost=0;
break;
}
}
elseif（j==Scan_End+2）
{
Left[i]=0;
L_last_lost=1;//中间有数据丢失标识位
break;
}
}//endfor（j=VisualMiddle[i-1];j>Scan_End;j--）
}//endelseif（L_last_lost）//识别到起始行但前一行丢失从虚拟中线开始循线个人收集整理勿做商业用途
/////////////////////////////////右边
if（Right_Start_Flag&&!
R_last_lost）//识别到起始行且前一行没有丢失个人收集整理勿做商业用途
{
Scan_End=R
升级会员 