main.c
来自「通过智能车上的CCD摄像头采集道路状况」· C语言 代码 · 共 1,391 行 · 第 1/3 页
C
1,391 行
char good=0;
unsigned char L1 = 0;
unsigned char L2 = 0;
unsigned char L3 = 0;
char flag1 = 0;
char flag2 = 0;
char flag3 = 0;
char flag6 = 0;
/////////////////
p = &Image[LineNum][0];
for(i=VIDEO_LEFT;i<VIDEO_RIGHT;i++){
if((*(p+i)-*(p+i+1))==0)
continue;
//没有沿就下一次循环
if(((*(p+i)-*(p+i+1))<0)&&(flag6==0)) {
L1 = i+1;
flag1 = 1;
continue;
}
if((flag1==1)&&(*(p+i)-*(p+i+1))>0) {
if((i-L1)<45&&(i-L1)>12) {
L2 = i+1;
flag2 = 1;
flag6=1;
}
////////////// //黑起跑线宽度
else if((i-L1)<13&&(i-L1)>1) {/////引导线宽度
good=1;
GuideLine[LineNum] = (L1+i)/2 ;
L2 = i+1;
flag2 = 1;
flag6=1;
}
flag1 = 0;
continue;
}
///////////////////////////////////////////
if((flag2==1)&&(*(p+i)-*(p+i+1))<0) {
L3 = i+1;
flag3 = 1;
flag2 = 0;
continue;
}
/////////////////////////////////////////
if((flag3==1)&&(*(p+i)-*(p+i+1))>0) {
if((i-L3)<13&&(i-L3)>1) {/////引导线宽度
good=1;
GuideLine[LineNum] = (L3+i)/2 ;
startflag=1; feng=LineNum;
break;
}
else if((i-L3)<45&&(i-L3)>12){
startflag=1; feng=LineNum;
break;
}
else{
flag2=1;
}
flag3 = 0;
}
}
//////////////////////////////////////////
if(good==0)
GuideLine[LineNum]=0;
}
//*******************Get far line**************************//
void FarLine(unsigned char row){ //黑为0，白为1
unsigned char *p;
unsigned char i;
char good=0;
unsigned char L1 = 0;
unsigned char L2 = 0;
unsigned char L3 = 0;
char flag1 = 0;
char flag2 = 0;
char flag3 = 0;
char flag6 = 0;
/////////////////
p = &Image[row][0];
for(i=VIDEO_LEFT;i<VIDEO_RIGHT;i++){
if((*(p+i)-*(p+i+1))==0)
continue;
//没有沿就下一次循环
if(((*(p+i)-*(p+i+1))<0)&&(flag6==0)) {
L1 = i+1;
flag1 = 1;
continue;
}
if((flag1==1)&&(*(p+i)-*(p+i+1))>0) {
if((i-L1)<40&&(i-L1)>10) {
L2 = i+1;
flag2 = 1;
flag6=1;
}
////////////// //黑起跑线宽度
else if((i-L1)<11&&(i-L1)>0) {/////引导线宽度
good=1;
GuideLine[row] = (L1+i)/2 ;
L2 = i+1;
flag2 = 1;
flag6=1;
}
flag1 = 0;
continue;
}
///////////////////////////////////////////
if((flag2==1)&&(*(p+i)-*(p+i+1))<0) {
L3 = i+1;
flag3 = 1;
flag2 = 0;
continue;
}
/////////////////////////////////////////
if((flag3==1)&&(*(p+i)-*(p+i+1))>0) {
if((i-L3)<11&&(i-L3)>0) {/////引导线宽度
good=1;
GuideLine[row] = (L3+i)/2 ;
startflag=1; feng=row;
break;
}
else if((i-L3)<40&&(i-L3)>10){
startflag=1; feng=row;
break;
}
else{
flag2=1;
}
flag3 = 0;
}
}
//////////////////////////////////////////
if(good==0)
GuideLine[row]=0;
}
/***************Guide line***************************/
unsigned char GetLine(void) { // //10---31
volatile unsigned char k;
unsigned char value;
unsigned int sum=0;
for(k=2;k<10;k++){
FarLine(k);
}
////////////////////////////////////////////////////////////
for(k=2;k<SAMPLELINE;k++){
if(GuideLine[k]<=VIDEO_LEFT||GuideLine[k]>=VIDEO_RIGHT)
ImageGood[k]=0;
else
ImageGood[k]=1; //先找出扫描过程提取失败的点
}
k=29;
while(ImageGood[k]==0&&ImageGood[k-1]==0&&ImageGood[k-2]==0&&k>8) k--;
if (k<10) {
return 0 ; //全场都失败返回0
}
bottom(); //对近处滤波并还原 第28，29肯定是好的
// 还原黑线并找top
k=27;
do{
if (ImageGood[k]==0)
GuideLine[k]=2*GuideLine[k+1]-GuideLine[k+2];
else {
if(GuideLine[k+1]-GuideLine[k+2]<0)
value= GuideLine[k+2]-GuideLine[k+1];
else
value=GuideLine[k+1]-GuideLine[k+2];
if(GuideLine[k]<(GuideLine[k+1]-value-9)||GuideLine[k]>(GuideLine[k+1]+value+9))
GuideLine[k]=2*GuideLine[k+1]-GuideLine[k+2];
}
k--;
}while (k>1 &&GuideLine[k+1]>VIDEO_LEFT&&GuideLine[k+1]<VIDEO_RIGHT) ;
top=k+1;
if(GuideLine[top]<=VIDEO_LEFT||GuideLine[top]>=VIDEO_RIGHT)
GuideLine[top++]=0;
//Find the top
LastAngle=angle;
if(top<12) {
angle=VIDEO_CENTER-((GuideLine[top]+GuideLine[top+1]+GuideLine[top+2]+GuideLine[top+3]+GuideLine[top+4]+GuideLine[top+5])/6);
}
//速度参数
if(top<8) {
AngleWay= GuideLine[top+1]-GuideLine[top+9];
if(AngleWay<0)
AngleWay=0-AngleWay;
}
///////////////////////////////////////////
if(top<12) {
errWay1=(GuideLine[top]+GuideLine[top+1]+GuideLine[top+2])/3-(GuideLine[top+11]+GuideLine[top+12])/2;
if(errWay1<0)
errWay1 = 0-errWay1;
errWay2=(GuideLine[top+11]+GuideLine[top+12])/2-(GuideLine[top+17]+GuideLine[top+18])/2;
if(errWay2<0)
errWay2 = 0-errWay2;
errWay = errWay1-errWay2;
}
//////////////////////////S Way
if(top==2) {
for(k=top;k<SAMPLELINE-6;k++){
if(GuideLine[k]-GuideLine[k+6]<0)
s1=1;
if(GuideLine[k]-GuideLine[k+6]>0)
s2=1;
}
}
return 1;
}
//*********************还原底部************************************//
void bottom (void) {
if(ImageGood[29]==0) {
if(ImageGood[28]==0){
if (ImageGood[27]==0){
if(ImageGood[26]==0){
if(ImageGood[25]==0){
if(ImageGood[24]==0) {
if(ImageGood[23]==0){
if(ImageGood[22]==0){
if(ImageGood[21]==0){
if(ImageGood[20]==0) {
if(ImageGood[19]==0){
if(ImageGood[18]==0)
GuideLine[18]=2*GuideLine[17]-GuideLine[16];
GuideLine[19]=2*GuideLine[18]-GuideLine[17];
}
GuideLine[20]=2*GuideLine[19]-GuideLine[18];
}
GuideLine[21]=2*GuideLine[20]-GuideLine[19];
}
GuideLine[22]=2*GuideLine[21]-GuideLine[20];
}
GuideLine[23]=2*GuideLine[22]-GuideLine[21];
}
GuideLine[24]=2*GuideLine[23]-GuideLine[22];
}
GuideLine[25]=2*GuideLine[24]-GuideLine[23];
}
GuideLine[26]=2*GuideLine[25]-GuideLine[24];
}
GuideLine[27]=2*GuideLine[26]-GuideLine[25];
}
GuideLine[28]=2*GuideLine[27]-GuideLine[26];
}//the 29 line
if (ImageGood[27]==0){
if(ImageGood[26]==0){
if(ImageGood[25]==0){
if(ImageGood[24]==0) {
if(ImageGood[23]==0){
if(ImageGood[22]==0){
if(ImageGood[21]==0){
if(ImageGood[20]==0) {
if(ImageGood[19]==0){
if(ImageGood[18]==0)
GuideLine[18]=2*GuideLine[17]-GuideLine[16];
GuideLine[19]=2*GuideLine[18]-GuideLine[17];
}
GuideLine[20]=2*GuideLine[19]-GuideLine[18];
}
GuideLine[21]=2*GuideLine[20]-GuideLine[19];
}
GuideLine[22]=2*GuideLine[21]-GuideLine[20];
}
GuideLine[23]=2*GuideLine[22]-GuideLine[21];
}
GuideLine[24]=2*GuideLine[23]-GuideLine[22];
}
GuideLine[25]=2*GuideLine[24]-GuideLine[23];
}
GuideLine[26]=2*GuideLine[25]-GuideLine[24];
}
GuideLine[27]=2*GuideLine[26]-GuideLine[25];
}
GuideLine[29]=2*GuideLine[28]-GuideLine[27];
}
///////////
else if(ImageGood[28]==0){
if (ImageGood[27]==0){
if(ImageGood[26]==0){
if(ImageGood[25]==0){
if(ImageGood[24]==0) {
if(ImageGood[23]==0){
if(ImageGood[22]==0){
if(ImageGood[21]==0){
if(ImageGood[20]==0) {
if(ImageGood[19]==0){
if(ImageGood[18]==0)
GuideLine[18]=2*GuideLine[17]-GuideLine[16];
GuideLine[19]=2*GuideLine[18]-GuideLine[17];
}
GuideLine[20]=2*GuideLine[19]-GuideLine[18];
}
GuideLine[21]=2*GuideLine[20]-GuideLine[19];
}
GuideLine[22]=2*GuideLine[21]-GuideLine[20];
}
GuideLine[23]=2*GuideLine[22]-GuideLine[21];
}
GuideLine[24]=2*GuideLine[23]-GuideLine[22];
}
GuideLine[25]=2*GuideLine[24]-GuideLine[23];
}
GuideLine[26]=2*GuideLine[25]-GuideLine[24];
}
GuideLine[27]=2*GuideLine[26]-GuideLine[25];
} GuideLine[28]=(GuideLine[27]+GuideLine[29])/2;
}//the 28 line
}
//**************************************************************//
void ServoControl(void){
unsigned char Potion;
Image_good=GetLine();
if(Image_good) {
if(top==2){
if(AngleWay<35)
servoScale=SERVO_CENTER+angle*5+(angle-LastAngle);
else
servoScale=SERVO_CENTER+angle*7+(angle-LastAngle)*4;
}
else if(top<12) {
Potion=Kp+top;
if(Potion>18)
Potion=18;
servoScale=SERVO_CENTER+angle*Potion+(angle-LastAngle)*Kd;
}
else { //imagegood==0
if (servoScale<SERVO_CENTER)
servoScale=SERVO_RIGHT;
if(servoScale>SERVO_CENTER )
servoScale=SERVO_LEFT;
}
if (servoScale>SERVO_LEFT) servoScale=SERVO_LEFT;
if (servoScale<SERVO_RIGHT) servoScale=SERVO_RIGHT;
}else { //imagegood==0
if (servoScale<SERVO_CENTER)
servoScale=SERVO_RIGHT;
if(servoScale>SERVO_CENTER )
servoScale=SERVO_LEFT;
}
PWMDTY23=servoScale;
}
/******************************Speed Control**************************************/
void SpeedControl0(void) { //ren gaosu
PTP_PTP0=0;
if(Image_good) {
if(top==2) {
if((AngleWay<4)||(s1==1&&s2==1)){
if(Samplespeed<110) //直，可加速
PWMDTY1=90;
if(Samplespeed>110){ // /减速
PTP_PTP0=1;
PWMDTY1=50;
}
s1=0;
s2=0;
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?