飞思卡尔 智能车(山大 Router) 核心源码

 这份代码比较完整，是一份高质量的程序。因为CSDN参加智能车比赛的朋友很多，就贴在这里了~

 （总体而言：算法经典，完整而不复杂。）

-------------------------------------------------------------------------------------------------------

/************************* //MAIN 主函数 *************************/ #include <hidef.h> /* common defines and macros */ #include "derivative.h" /* derivative-specific definitions */ #include "Init.h" #include "Sd_System.h" # define BlackMid 134 //图像的中心与边缘 # define LeftEdge 37 # define RightEdge 233 # define ServoMid 2850 # define ServoMax 400 //************************************************ /****************************************** // 可调整的变量 *************************************************/ //************************************************** uchar EptWith[276]= //原始预计线宽Camera Relative { //与摄像头高度和角度,焦距相关的量，用//Camera Relative 在 后面注释，换时搜索调整 20,20,20,20,20,20,20,20,20,20, 20,20,20,20,20,20,20,20,20,20, 20,20,20,20,20,20,20,20,20,20, 19,19,19,19,19,19,19,19,19,19, 19,19,19,19,19,19,19,19,19,19, 19,19,19,19,19,19,19,19,19,19, 18,18,18,18,18,18,18,18,18,18, 18,18,18,18,18,18,18,18,18,18, 18,18,18,18,18,18,18,18,18,18, 17,17,17,17,17,17,17,17,17,17, 17,17,17,17,17,17,17,17,17,17, 17,17,17,17,17,17,17,17,17,17, 16,16,16,16,16,16,16,16,16,16, 16,16,16,16,16,16,16,16,16,16, 16,16,16,16,16,16,16,16,16,16, 15,15,15,15,15,15,15,15,15,15, 15,15,15,15,15,15,15,15,15,15, 15,15,15,15,15,15,15,15,15,15, 14,14,14,14,14,14,14,14,14,14, 14,14,14,14,14,14,14,14,14,14, 14,14,14,14,14,14,14,14,14,14, 13,13,13,13,13,13,13,13,13,13, 13,13,13,13,13,13,13,13,13,13, 13,13,13,13,13,13,13,13,13,13, 12,12,12,12,12,12,12,12,12,12, 12,12,12,12,12,12,12,12,12,12, 12,12,12,12,12,12,12,12,12,12, 11,11,11,11,11,11 }; void ChangeEptWith() //方便的修改原始预计线宽Camera Relative { int i; for(i=0;i<275;i++) EptWith[i]=EptWith[i]-5; } uchar RelEptWith; //计算后的实际预计线宽。公式： ucharWidthAllowErr=8; //黑线宽度允许误差的基础值Camera Relative uchar LineScanWith=10; //原始的下一行的扫描范围Camera Relative uchar RelScanWith; //计算后的实际扫描范围 int UsableLines=0,ValidLine=0; /*************************************** //图像数据区 ********************************************************/ uchar v_point_up[276][5]; //存放上升沿与下降沿的原始数据数组 uchar v_point_down[276][5]; uchar g_BlackCenter[276]; //提取的黑线中心值 uchar g_Width[276]; //宽度 uchar ValidFlag[276]; //行有效标志 uchar his_BlackCenter[276]; //历史值 /************************************* //其他变量 *****************************************/ uint Flag_Down=0,Flag_Up=0; uint LineCCD = 0; int Line_Save=0,Line_Process=0; uchar FlagField=0; uchar PITCNT0temp; int UsableLines0; int ValidLine0; int Kp; int Servo=2850; int DC_Duty=100; int Pulse_Count0=0; //测速用 int Pulse_Count1=0; int Pulse_Width=0; int AveSpeed=0; /******************************************* // 函数声明 ****************************************************/ void ProcessLineOne(void); void ProcessLineOthers(void); void LineProcess(void); void MeasureSpeed(void); void FindBlackLine(void); void StartLineSeek(void); /******************************************************* // 舵机 速度控制 *************************************************************/ void Turn(void) { int BlackErr; Kp=6-UsableLines0/90; if(ValidLine0>120) BlackErr=g_BlackCenter[120] - BlackMid; else BlackErr=g_BlackCenter[ValidLine0] - BlackMid; Servo = ServoMid + Kp*BlackErr; if(Servo>(Servo+ServoMax)) Servo = Servo + ServoMax; else if(Servo<(Servo-ServoMax)) Servo = Servo - ServoMax; PWMDTY01 = Servo; } void SpeedCtrl(void) { PWMDTY45=DC_Duty; } //************************************************ // 程序入口点 main() //************************************************ void main(void) { setbusclock_64M(); //一堆初始化 PORT_Init(); SD_System_Init(); ECT_Init(); PIT_Init(); Servo_Init(); PAC_Init(); Forward_Init(); EnableInterrupts; ChangeEptWith(); //Easy to Change expectWidth for(;;) { if(Line_Process<Line_Save) LineProcess(); if(Line_Process==276&&FlagField==0) //执行舵机和调速控制 { UsableLines0=UsableLines; ValidLine0=ValidLine; //防止控制时中断改变，BDM 里也稳定 FlagField=1; MeasureSpeed(); //测速 Turn(); SpeedCtrl(); StartLineSeek(); } _FEED_COP(); } } /******************************************** // 行处理 ********************************************************/ void LineProcess(void) { if(Line_Process==0) ProcessLineOne(); else ProcessLineOthers(); if(Line_Process <= 50 && field ==0) FindBlackLine(); Line_Process++; } void ProcessLineOne(void) //第一行处理 { uchar cnt,i,j; uchar temp[5]; //暂存 cnt=0; for(i=0;i<4&&v_point_up[Line_Process][i]!=0&&v_point_down[Line_Process][i+1]!=0;i++) { if(v_point_up[Line_Process][i]-v_point_down[Line_Process][i+1]>=11&&v_point_up[Line_Process][i]-v _point_down[Line_Process][i+1]<=22) //第一行的左右值 { temp[cnt]=i; //记录的是指针 cnt++; } } if(cnt==0) //process edge,depend on history { //Camera Relative 40 about 1/5 of 192 if(his_BlackCenter[Line_Process]<LeftEdge+40) g_BlackCenter[Line_Process]=LeftEdge; //边沿的认定范围 else if(his_BlackCenter[Line_Process]>RightEdge-40) g_BlackCenter[Line_Process]=RightEdge; else g_BlackCenter[Line_Process]=his_BlackCenter[Line_Process]; //使用历史值 g_Width[Line_Process]=EptWith[Line_Process]; //没有提取出准确地，用预计值 ValidFlag[Line_Process]=0; } else if(cnt==1) //it must be the black line { g_BlackCenter[Line_Process]=(v_point_down[Line_Process][temp[cnt-1]+1]+v_point_up[Line_Process][ temp[cnt-1]])/2; g_Width[Line_Process]=v_point_up[Line_Process][temp[cnt-1]]-v_point_down[Line_Process][temp[cnt-1] +1]; ValidFlag[Line_Process]=1; ValidLine=Line_Process; UsableLines++; } else //有多个时,缩小条件选取 { j=0; for(i=0;i<cnt;i++) if(v_point_up[Line_Process][i]-v_point_down[Line_Process][i+1]>=13&&v_point_up[Line_Process][i]-v _point_down[Line_Process][i+1]<=20)//Camera Relative { g_BlackCenter[Line_Process]=(v_point_down[Line_Process][temp[cnt-1]+1]+v_point_up[Line_Process][ temp[cnt-1]])/2; g_Width[Line_Process]=v_point_up[Line_Process][temp[cnt-1]]-v_point_down[Line_Process][temp[cnt-1] +1]; j++; ValidFlag[Line_Process]=1; ValidLine=Line_Process; UsableLines++; } if(j==0) { g_BlackCenter[Line_Process]=his_BlackCenter[Line_Process]; //使用历史值 g_Width[Line_Process]=EptWith[Line_Process]; ValidFlag[Line_Process]=0; } } his_BlackCenter[Line_Process]=g_BlackCenter[Line_Process];//Save history value } void ProcessLineOthers(void) //非第一行处理 { int i,j,temp,temp1,temp2; //首先确定本行黑线宽度和扫描范围 RelEptWith=EptWith[Line_Process]*g_Width[Line_Process-1]/EptWith[Line_Process-1]; RelScanWith=LineScanWith*g_Width[Line_Process-1]/EptWith[Line_Process-1]; WidthAllowErr=5*g_Width[Line_Process-1]/EptWith[Line_Process-1]; if(RelEptWith-WidthAllowErr<=1) RelEptWith=WidthAllowErr+1; temp=g_BlackCenter[Line_Process-1]; //以上一行的中心为基准，左右寻找 for(i= 0;i<RelScanWith;i++) { temp1 = temp + i - 1; temp2 = temp - i; if(temp1>RightEdge) temp1=RightEdge; //嵌位 if(temp2<LeftEdge) temp2=LeftEdge; for(j=0;j<4;j++) //下降沿的第一个和上升沿的最后一个不考虑 { if(temp1==v_point_up[Line_Process][j]) { g_Width[Line_Process]=v_point_up[Line_Process][j]-v_point_down[Line_Process][j+1]; if(g_Width[Line_Process]>RelEptWith-WidthAllowErr&&g_Width[Line_Process]<RelEptWith+WidthAll owErr+3) //易大不宜小 { g_BlackCenter[Line_Process]=(v_point_up[Line_Process][j]+v_point_down[Line_Process][j+1])/2; ValidFlag[Line_Process]=1; ValidLine=Line_Process; UsableLines++; break; } } else if(temp2==v_point_up[Line_Process][j]) { g_Width[Line_Process]=v_point_up[Line_Process][j]-v_point_down[Line_Process][j+1]; if(g_Width[Line_Process]>RelEptWith-WidthAllowErr&&g_Width[Line_Process]<RelEptWith+WidthAll owErr+3) //易大不宜小故 { g_BlackCenter[Line_Process]=(v_point_up[Line_Process][j]+v_point_down[Line_Process][j+1])/2; ValidFlag[Line_Process]=1; ValidLine=Line_Process; UsableLines++; break; } } else if(temp1==v_point_down[Line_Process][j+1]) { g_Width[Line_Process]=v_point_up[Line_Process][j]-v_point_down[Line_Process][j+1]; if(g_Width[Line_Process]>RelEptWith-WidthAllowErr&&g_Width[Line_Process]<RelEptWith+WidthAll owErr+3) //易大不宜小故 { g_BlackCenter[Line_Process]=(v_point_up[Line_Process][j]+v_point_down[Line_Process][j+1])/2; ValidFlag[Line_Process]=1; ValidLine=Line_Process; UsableLines++; break; } } else if(temp2==v_point_down[Line_Process][j+1]) { g_Width[Line_Process]=v_point_up[Line_Process][j]-v_point_down[Line_Process][j+1]; if(g_Width[Line_Process]>RelEptWith-WidthAllowErr&&g_Width[Line_Process]<RelEptWith+WidthAll owErr+3) //易大不宜小故 { g_BlackCenter[Line_Process]=(v_point_up[Line_Process][j]+v_point_down[Line_Process][j+1])/2; ValidFlag[Line_Process]=1; ValidLine=Line_Process; UsableLines++; break; } } else continue; } if(ValidLine==Line_Process) break; //找到了跳出 } if(ValidLine!=Line_Process) //如果没找到 { if(g_BlackCenter[Line_Process-1]<LeftEdge+2*EptWith[Line_Process]) g_BlackCenter[Line_Process]=LeftEdge; else if(g_BlackCenter[Line_Process-1]>RightEdge-2*EptWith[Line_Process]) g_BlackCenter[Line_Process]=RightEdge; else g_BlackCenter[Line_Process]=g_BlackCenter[Line_Process-1]; // 使用上一行值 his_BlackCenter[Line_Process]; //使用历史值 g_Width[Line_Process]=RelEptWith; //没有提取出准确地，用预计值 ValidFlag[Line_Process]=0; } his_BlackCenter[Line_Process]=g_BlackCenter[Line_Process]; } void MeasureSpeed(void) { Pulse_Count0=PACNT; Pulse_Width=Pulse_Count0-Pulse_Count1; AveSpeed=33*Pulse_Width/20; //轮子转过3.3cm 产生100 个脉冲，公式为（（3.3/100）*Pulse_Width） /0.02 (cm/s) Pulse_Count1=Pulse_Count0; } //************************************************** /******************************************* // 中断处理程序 *4 上升沿 下降沿 行 场 *************************************************/ //************************************************ void interrupt 8 Timer0_ISR(void) //上升沿中断 { PITCNT0temp=PITCNT0; if(Flag_Down - Flag_Up == 1)//PITCNT0temp>38 { v_point_up[Line_Save][Flag_Up] = PITCNT0temp; //上升沿中断记录计数器值 Flag_Up ++; //此行有上升沿中断点加一 } TFLG1_C0F = 1; //清标志 } void interrupt 9 Timer1_ISR(void) //下降沿中断 { PITCNT0temp=PITCNT0; if(Flag_Down - Flag_Up == 0)//&&PITCNT0temp<232 { v_point_down[Line_Save][Flag_Down] =PITCNT0temp; //上升沿中断记录计数器值 Flag_Down ++; //此行有上升沿中断点加一 } TFLG1_C1F = 1; //清标志 } int Up_Lost=0; int Down_Lost=0; int LostLine=0; void interrupt 56 PP5_ISR(void) //行中断 { PIFP_PIFP5 = 1; //行中断 PITCFLMT_PFLMT0=1; PITFLT_PFLT0=1; //初始化计数器值,行中断之后要保证处理时间小于（255-235）*16=320 个时钟周期 //测试用 if(Flag_Down>Flag_Up) { Up_Lost++; LostLine=Flag_Down; } if(Flag_Down<Flag_Up) { Down_Lost++; LostLine=Flag_Up; } Flag_Down = 0; Flag_Up = 0; LineCCD++; field = 0; if(LineCCD>=24&&LineCCD<300) //在24 行到300 行之间，每1 行取一行 { if(LineCCD==24) //一场开始时清零 { Line_Save=0; Line_Process=0; FlagField=0; UsableLines=0; ValidLine=0; TIE = TIE_C0I_MASK|TIE_C1I_MASK;//打开通道0 和1 的中断interrupt 8，9 } else Line_Save++; v_point_up[Line_Save][0]=0; v_point_up[Line_Save][1]=0; //清零，0 不须清零，必有 v_point_up[Line_Save][2]=0; v_point_up[Line_Save][3]=0; v_point_up[Line_Save][4]=0; v_point_down[Line_Save][0]=0; v_point_down[Line_Save][1]=0; v_point_down[Line_Save][2]=0; v_point_down[Line_Save][3]=0; v_point_down[Line_Save][4]=0; } if(LineCCD== 300) { Line_Save=276; PIEP_PIEP5=0; //关行中断 LineCCD=0; TIE &=~((1<< TIE_C0I_MASK)|(1<<TIE_C1I_MASK)); //关闭打开通道0 和1 的中断 } } void interrupt 24 PJ7_ISR(void) //场中断 { static int temp= 0; PIFJ_PIFJ7 = 1; PIEP_PIEP5 = 1;//开行中断 if(temp == 0) //每一帧的两场都取 { temp = 1; PPSJ_PPSJ7 = 0;//下跳沿中断 PIEJ_PIEJ7 = 1;//中断使能 } else { temp = 0; PPSJ_PPSJ7 = 1;//上跳沿中断 PIEJ_PIEJ7 = 1;//中断使能 } }