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

最新推荐文章于 2025-07-03 13:39:37 发布

转载最新推荐文章于 2025-07-03 13:39:37 发布 · 2.5k 阅读

文章标签：

#up #timer #macros #system #算法 #c

飞思卡尔智能车比赛专栏收录该内容

5 篇文章

订阅专栏

本文分享一份完整的高质量智能车竞赛代码，包含主函数、参数调整、图像数据处理、舵机速度控制等关键部分，旨在提供给参加智能车比赛的朋友参考。

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

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

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

/************************* //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;//中断使能 } }