#include?“baseInc.h“
#include?“math.h“
/*========================================================//
??函數(shù)：PIDInit
??功能：PID參數(shù)初始化?
??作者：
??參數(shù)：
??		?
??返回值：
??備注：
/==========================================================*/

void?PIDInit（void）
{
	sPID.En_1?=?0;
	sPID.En_2?=?0;
	sPID.Un_1?=?0;
	sPID.Kp???=?4;
	sPID.Ki???=?0;//0.001;//0.014;
	sPID.Kd???=?1;
}

/*========================================================//
??函數(shù)：PIDCalc
??功能：PID算法?
??作者：
??參數(shù)：?PID?*?pp?:PID結(jié)構(gòu)體
??		?SetPoint?:設(shè)定目標(biāo)值
??		?DetectPoint：檢測(cè)值、
??返回值：Un?uint?:輸出控制電機(jī)量
??備注：
/==========================================================*/?

float?PIDCalc（?PID?*ppint?SetPoint?int?DetectPoint?）
{
	int?En;?????????????????//本次偏差
	float?Un;
			//輸出
	En?=?SetPoint?-?DetectPoint;?//?偏差
???
	Un?=?pp->Un_1?+pp->Kp?*?（En?-?pp->En_1）
	????	+?pp->Ki?*?En?//?積分項(xiàng)
			?+?pp->Kd?*?（En?-?2*pp->En_1?+?pp->En_2）;?//?微分
	pp->En_2?=?pp->En_1;
	pp->En_1?=?En;
??pp->Un_1?=?Un;

	return?Un;
}?

/*========================================================
LineEdgeDetect
一條水平線上的區(qū)域邊緣檢測(cè)
乘以因子?【-1?0?+1】
==========================================================*/
void?LineEdgeDetect（uchar?*imagechar?*resultchar?No）
{
??uchar?ij;
??for（i=1;?i??{
????
????result[i-1]?=?m_aImage[i+1][No]?-?m_aImage[i-1][No];
??}
}
/*=======================================================
GetLineCenter
提取道路中心線
=========================================================*/
char?GetLineCenter（char?*Lineuchar?*markcenteruchar?No）
{
??int?markl[3]markr[3];
??uchar?markweith;
??uchar?flaglflagr;
??int?center;
??uchar?i;
??flagl?=?0;
??flagr?=?0;
????for（i=0;?i????{
??????if（（m_aLineDot[i]<-6）&&（m_aLineDot[i+1]<-6））
??????{
????????flagl?=?1;
????????markl[0]?=?i;
??????}else?if（（m_aLineDot[i]>6）&&（m_aLineDot[i+1]>6））
??????{
????????markr[0]?=?i;
????????flagr?=?1;
????????break;
??????}
????}
????
??????
????if（flagl&&flagr）
????{
??????markweith?=?markr[0]?-?markl[0];
??????//m_cRoadLenth[m++]?=??markweith;
??????*markcenter?=?markweith/2+markl[0];?
????}else?if（flagl&&（!flagr））
????{
??????center?=?markl[0]+15-No;
??????*markcenter?=?center;
??????if（center>CCD_ROWS-1）（*markcenter）?=?CCD_ROWS-1;
????}else?if（（!flagl）&&flagr）
????{
??????center?=?markr[0]-15+No;
??????*markcenter?=?center;
??????if（center<0）（*markcenter）?=?0;
????}else
????{
??????return?NON;
????}
???return?YES;
????
??}
/*========================================================
RoadCenterDetect
路線標(biāo)識(shí)的中心線檢測(cè)
==========================================================*/

void?RoadCenterDetect（void）
{
??uchar?ijkmarkweithmarkcenter;
??uchar?col;
?
??m_aRoadParam.ValidStart?=?DISVALID;
??m_aRoadParam.ValidNum?=?0;
??
??for（col=0;?col??{
????LineEdgeDetect（m_aImagem_aLineDotcol）;
????if（GetLineCenter（&m_aLineDot&markcentercol）==YES）
????{
??????if（m_aRoadParam.ValidStart?==?DISVALI

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件?????197224??2010-03-27?22:42??Car\bin\P&E_ICD.abs

?????文件???????3018??2010-03-27?22:42??Car\bin\P&E_ICD.abs.phy

?????文件???????2980??2010-03-27?22:42??Car\bin\P&E_ICD.abs.s19

?????文件?????123807??2010-03-27?22:42??Car\bin\P&E_ICD.map

?????文件?????197224??2010-04-01?23:20??Car\bin\Simulator.abs

?????文件???????3022??2010-04-01?23:20??Car\bin\Simulator.abs.phy

?????文件???????2984??2010-04-01?23:20??Car\bin\Simulator.abs.s19

?????文件?????123809??2010-04-01?23:20??Car\bin\Simulator.map

?????文件?????????92??2010-03-27?22:42??Car\bin\P&E_ICD.xpr

?????文件?????141710??2010-04-01?23:21??Car\Car07.mcp

?????文件?????195780??2007-01-29?22:08??Car\Car070121\bin\P&E_ICD.abs

?????文件???????5040??2007-01-29?22:08??Car\Car070121\bin\P&E_ICD.abs.phy

?????文件???????5000??2007-01-29?22:08??Car\Car070121\bin\P&E_ICD.abs.s19

?????文件?????129934??2007-01-29?22:08??Car\Car070121\bin\P&E_ICD.map

?????文件?????????84??2007-01-29?22:10??Car\Car070121\bin\P&E_ICD.xpr

?????文件?????190660??2007-01-25?16:52??Car\Car070121\bin\Simulator.abs

?????文件???????3906??2007-01-25?16:52??Car\Car070121\bin\Simulator.abs.phy

?????文件???????3870??2007-01-25?16:52??Car\Car070121\bin\Simulator.abs.s19

?????文件?????127842??2007-01-25?16:52??Car\Car070121\bin\Simulator.map

?????文件?????176151??2007-01-25?16:55??Car\Car070121\Car07.mcp

?????文件???????4196??2007-01-16?08:09??Car\Car070121\Car071_Data\CWSettingsWindows.stg

?????文件??????69167??2007-01-16?08:09??Car\Car070121\Car071_Data\P&E_ICD\TargetDataWindows.tdt

?????文件??????69171??2007-01-16?08:09??Car\Car070121\Car071_Data\Simulator\TargetDataWindows.tdt

?????文件???????4380??2007-01-29?22:18??Car\Car070121\Car07_Data\CWSettingsWindows.stg

?????文件??????23427??2007-01-29?22:08??Car\Car070121\Car07_Data\P&E_ICD\objectCode\Arithmetic.c.o

?????文件???????8392??2007-01-29?21:53??Car\Car070121\Car07_Data\P&E_ICD\objectCode\datapage.c.o

?????文件???????7398??2007-01-29?21:56??Car\Car070121\Car07_Data\P&E_ICD\objectCode\Globe.c.o

?????文件??????13942??2007-01-29?21:54??Car\Car070121\Car07_Data\P&E_ICD\objectCode\InterruptVector.c.o

?????文件???????7568??2007-01-29?21:53??Car\Car070121\Car07_Data\P&E_ICD\objectCode\main.c.o

?????文件?????155264??2007-01-29?21:53??Car\Car070121\Car07_Data\P&E_ICD\objectCode\mc9s12dg128.c.o

............此處省略127個(gè)文件信息