摄像头组很稳定的黑线提取算法.docx

/本程序对黑线提取做了很大的改进,未对十字弯处理/*/本程序中加入了一些用于显示车模状态的LED灯/*经实测发现，近处的赛道宽度为60左右，远处为30左右，所以用可变赛道宽度进行搜索*/#include /* common defines and macros */#include derivative.h /* derivative-specific definitions */#include /-函数声明-/void delay(unsigned int t); #define HighSpeedLimit 40#define LowSpeedLimit 16#define SteerLeftLimit -35#define SteerRightLimit 35#define COLUMN 90 /采集列数 #define MID_COLUMN 45 / 中间黑线#define ROW 40/采集行数 #define LeftLED PORTB_PB0/左转方向灯#define RightLED PORTB_PB1/右转方向灯#define SpeedUpLED PORTB_PB2/加速指示灯#define SlowDownLED PORTB_PB3/减速方向灯 #define Crossing_RoadLED PORTB_PB4/十字弯#define Dashed_RoadLED PORTB_PB5/虚线路段#define Mid_Route_Width_Factor 0.48 /赛道宽度系数int steer_dire_label=0;int SteerDelta=0;/舵机最终的偏转增量unsigned int Speed=0;/显示当前PWM2占空比大小unsigned int PreSpeed=0;float Threshold_Factor=0.9; /阈值系数设置unsigned int Threshold=120; /初设动态阈值为90,以后每传来一帧数据更新一次float Kp=0.8;/舵机方向比例系数float Kd=5.0;/舵机方向微分系数float MotorSpeed_Factor=6.0;/马达控制unsigned char Image_DataROWCOLUMN;unsigned int LeftROW,RightROW;/左右黑线unsigned int VisualMiddleROW;/虚拟中线unsigned int MiddleROW;/最终存放中间黑线值的二维数组unsigned int Row_AttributeROW;/行属性unsigned int row,column; int m=0;/计算采集到的行数unsigned char Line_Flag=0; /奇偶场 unsigned int Line_C=0; /采集行数d int PreSteerDirection=50;/之前的舵机方向,用与前后比较unsigned int LeftFlag=0,RightFlag=0;/左右黑线标志unsigned int Left_Start_Flag=0,Right_Start_Flag=0;/左右起始黑线找到标志unsigned int L_lost_count=0;/左黑线丢失计数unsigned int R_lost_count=0;/右黑线丢失计数unsigned int L_last_lost=0;/左行上一行丢线标志unsigned int R_last_lost=0;/右行上一行丢线标志unsigned int L_last_memory=0;/左行上一次有黑线的黑线所在列数unsigned int R_last_memory=0;/右行上一次有黑线的黑线所在列数unsigned int HS_DataROW=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;unsigned int HS_Pointer=0;/指向行数数组中的数据int error2=0,0;/舵机PD调节时的误差参数void delay(unsigned int Time) /一般,Time设为10000,可以实现1秒一次 int i,j; for(i=0;iTime;i+) for(j=0;j8;j+) void SlowDown(int timer) int i; PWMDTY2=0; PWMDTY3=50; for(i=0;itimer;i+) _asm(nop); PWMDTY3=0; PWMDTY2=Speed;/* * 函数名称: 图像灰度值采集* 功能描述: 采集像素值 * 输 入: 无 * 输 出: 无 * 说明： */void Data_collect(void) int ia=0; while(ia /* 函数名称: PLL_Init* 功能描述: 时钟初始化函数* 说明:BUS CLOCK=80M*/void PLL_Init(void) /pllclock=2*osc*(1+SYNR)/(1+REFDV); CLKSEL=0x00;/disengage PLL to system PLLCTL_PLLON=1;/turn on PLL SYNR =0xc0 | 0x09; REFDV=0x80 | 0x01; POSTDIV=0x00; /pllclock=2*osc*(1+SYNR)/(1+REFDV)=160MHz; while(!(CRGFLG_LOCK=1); /when pll is steady ,then use it; CLKSEL_PLLSEL =1; /engage PLL to system; /*/* P0输出频率为300Hz的方波,用于控制舵机 */* P2,P3输出频率为20KHZ,用于驱动电机的转速 */*/void PWM_Init(void) PWME_PWME0=0x00; /禁止P0 PWME_PWME2=0x00; /禁止P2 PWME_PWME3=0x00; /禁止P3 PWMPOL_PPOL0=1;/PWM Polarity 0开始输出高电平.PWMPOL_PPOL2=1;/PWM Polarity 2开始输出高电平.PWMPOL_PPOL3=1;/PWM Polarity 3开始输出高电平.PWMPRCLK=0x43; /0100 0011 A=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* 功能描述: 行场中断初始化函数* 说明: */void TIM_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*/void SCI0_Init() SCI0BDL = (byte)(80000000 /* OSC freq */1) / 57600 /* baud rate */ / 16 /*factor*/); SCI0CR1 = 0X00; /*normal,no parity*/ SCI0CR2 = 0X0C; /*RIE=1,TE=1,RE=1, */* * 函数名称: 串口发射端程序* 功能描述: 发送赛道信息 1为黑线 0为白板* 输 入: 无 * 输 出: 无 * 说明： */void SCI0_Transmit(unsigned char data) while (!(SCI0SR1&0x80);/如果SCI状态寄存器SCI1SR1的TDRE位为0,则一直执行这个无语句的循环,否则,执行下面语句 SCI0DRL = data;/把要发送的数据存在SCI数据寄存器的低8位中 /* 函数名称: IO_Init* 功能描述: 初始化函数* 说明: */ void IO_Init(void) DDRA=0X00;/A端口的数据传送方向为输入 DDRB=0XFF;/端口B为输出 PORTB=0XFF;/端口B全部打开/*/* 提取出图像的基本特征*/*/ void Image_Operates() char i,j; char Scan_Start,Scan_End;/扫描起始点，扫描终止点 char Gate,Range; unsigned char Temp_Mid_Route; int t,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-) /数据初始化 Lefti=0; Righti=COLUMN; VisualMiddlei=0; Row_Attributei=0; for(i=ROW-1;i=0;i-) if(i30) Gate=45; Range=40; else if(i20) Gate=35; Range=35; else if(i10) Gate=25; Range=30; else Gate=20; Range=20; LeftFlag=RightFlag=0;/左右黑线标志位清零 if(i=ROW-2)/前两行全局扫描 /左边黑线起始行提取 for(j=MID_COLUMN;j1;j-) if(Image_DataijThreshold/*&Image_Dataij-1Gate)|(Image_Dataij+2-Image_DataijGate) Lefti=j; Left_Start_Flag=1;/识别到黑线 LeftFlag=1; break; else if(j=2) Lefti=0; break; /end for(j=MID_COLUMN;j1;j-) /右边黑线起始行提取 for(j=MID_COLUMN;jCOLUMN-1;j+) if(Image_DataijThreshold/*&Image_Dataij+1Gate)|(Image_Dataij-2-Image_DataijGate) Righti=j; Right_Start_Flag=1;/识别到黑线 RightFlag=1; break; else if(j=COLUMN-2) Righti=COLUMN; break; /end for(m=MID_COLUMN;mCOLUMN-1;m+) /end if(i2) /前两行全局扫描 else / (iROW-2) /左边 if(Left_Start_Flag&!L_last_lost) /识别到起始行且前一行没有丢失 Scan_End=Lefti+1-Range; if(Scan_EndVisualMiddlei+1) Scan_Start=VisualMiddlei+1; for(j=Scan_Start;jScan_End;j-) if(Image_DataijThreshold/*&Image_Dataij-1Gate)|(Image_Dataij+2-Image_DataijGate) Lefti=j; LeftFlag=1;/识别到黑线 break; else if(j=(Scan_End+1) Lefti=0; L_last_lost=1;/这一行数据丢失标识位 L_last_memory=Lefti+1; break; /end if(Left_Start_Flag&!L_last_lost) /识别到起始行且前一行没有丢失 else if(!Left_Start_Flag) /没有识别到起始行，继续寻找起始行 for(j=VisualMiddlei+1;j1;j-) if(Image_DataijThreshold/*&Image_Dataij-1Gate)|(Image_Dataij+2-Image_DataijGate) Lefti=j; Left_Start_Flag=1;/识别到黑线 LeftFlag=1; break; else if(j=2) Lefti=0; break; /end for(j=MID_COLUMN;j1;j-) /end else if(!Left_Start_Flag) /没有主识别到起始行，继续寻找起始行 else if(L_last_lost) /识别到起始行但前一行丢失从虚拟中线开始循线 Scan_End=L_last_memory-Range; if(Scan_EndScan_End;j-) if(Image_DataijThreshold/*&Image_Dataij-1Gate)|(Image_Dataij+2-Image_DataijGate) Lefti=j; LeftFlag=1; L_last_lost=0; break; else if(j=Scan_End+2) Lefti=0; L_last_lost=1;/中间有数据丢失标识位 break; / end for(j=VisualMiddlei-1;jScan_End;j-) / end else if(L_last_lost) /识别到起始行但前一行丢失从虚拟中线开始循线 /右边 if(Right_Start_Flag&!R_last_lost) /识别到起始行且前一行没有丢失 Scan_End=Righti+1+Range; if(Scan_EndCOLUMN) Scan_End=COLUMN; Scan_Start=Righti+1-Range; if(Scan_StartVisualMiddlei+1) Scan_Start=VisualMiddlei+1; for(j=Scan_Start;jScan_End;j+) if(Image_DataijThreshold/*&Image_Dataij+1Gate)|(Image_Dataij-2-Image_DataijGate) Righti=j; RightFlag=1;/识别到黑线 break; else if(j=(Scan_End-2) Righti=COLUMN; R_last_lost=1;/这一行数据丢失标识位 R_last_memory=Righti+1; break; /end if(R_Start_Flag&!R_last_lost) else if(!Right_Start_Flag) /没有别到右起始行，继续寻找起始行 for(j=VisualMiddlei+1;jCOLUMN-1;j+) if(Image_DataijThreshold/*&Image_Dataij+1Gate)|(Image_Dataij-2-Image_DataijGate) Righti=j; Right_Start_Flag=1;/识别到黑线 RightFlag=1; break; else if(j=COLUMN-2) Righti=COLUMN; break; /end for(j=VisualMiddlei-1;jCOLUMN) Scan_End=COLUMN; for(j=VisualMiddlei+1;jScan_End-1;j+) if(Image_DataijThreshold/*&Image_Dataij+1Gate)|(Image_Dataij-2-Image_DataijGate) Righti=j; RightFlag=1; R_last_lost=0; break; else if(j=COLUMN-2) Righti=COLUMN; R_last_lost=1;/中间有数据丢失标识位 break; / end for(j=VisualMiddlei-1;j2) /属性判断 if(LeftFlag&RightFlag) Row_Attribute i=1;/两边都有黑线 else if(LeftFlag) Row_Attribute i=2;/左边有黑线 else if(RightFlag) Row_Attribute i=3;/左边丢失,仅右边有黑线 else Row_Attribute i=0; /两边丢失 /中线读取 if(i=ROW-1) VisualMiddlei=MID_COLUMN; else /要知道，最前面还有一个大大的for(n=0;nRows;n+)循环语句 if(Row_Attribute i) VisualMiddlei=(int)(Righti+Lefti)/2; else VisualMiddlei=VisualMiddlei+1; /end of for(i=0;i=0;t-) if(Row_Attributet=1) Middlet=(Leftt+Rightt)/2; if(Row_Attributet=2)/只有左行 Temp_Mid_Route=(int)(16+Mid_Route_Width_Factor*t); Rightt=Leftt+2*Temp_Mid_Route; Middlet=Leftt+Temp_Mid_Route; else if(Row_Attributet=3)/只有右行 Temp_Mid_Route=(int)(16+Mid_Route_Width_Factor*t); Leftt=Rightt-2*Temp_Mid_Route; Middlet=Rightt-Temp_Mid_Route; /第一行没有数据，利用以后行进行补 if(!Row_AttributeROW-1|!Row_AttributeROW-2) for(t=ROW-1;t0;t-) if(Row_Attributet) temp0=t; break; for(t=temp0;t0;t-) if(Row_Attributet=0) Middlet=Middlet+1; Row_Attributet=1; /给Image_Data赋值 for(row=0;row=16;i-) /仅对近处的20行取平均值 SteerSum+=Middlei-COLUMN/2; error1=(int)(SteerSum/20); SteerDelta=Kp*error1+Kd*(error1-error0); if(SteerDelta0) RightLED=1; LeftLED=0; if(SteerDeltaSteerRightLimit)/防止舵机抱死 PWMDTY0=50+SteerRightLimit; else if(SteerDeltaSteerLeftLimit) PWMDTY0=50+SteerLeftLimit; else PWMDTY0=50+SteerDelta; steer_dire_label=PWMDTY0; /马达速度控制/*/* 对采集的前20行处理,用来控制马达速度 */*/void MotorSpeedCtl() int Sum=0; int i=0; int Front_Black_Line_Warp=0; SlowDownLED=SpeedUpLED=0; /对前20行处理,判断是弯道,S弯等,用以控制电机速度 /对黑直线上下界限制 for(i=10;i30;i+) Front_Black_Line_Warp+=(Middlei-COLUMN/2);/取远处黑线与黑直线之间的偏差值 if(Front_Black_Line_Warp0) Front_Black_Line_Warp=-Front_Black_Line_Warp; /我们现在要的只是它偏离的绝对值 Speed=(int)(MotorSpeed_Factor*500/(Front_Black_Line_Warp)*(Front_Black_Line_Warp); if(Speed=HighSpeedLimit) PWMDTY2=HighSpeedLimit; e