#include?“includes.h“
#include?“define.h“

?typedef?signed?int?Loopstack;

/*----------------------------------------PID部分------------------------------------*/??
#define?P???16
#define?I???0
#define?D???-3
#define?SP???0
#define?SI???11
#define?SD???0
#define?A????SP+SI+SD
#define?B????SP+2*SD
#define?C????SD


typedef?struct?pid???????????????????//pid調節結構
{
??Loopstack?Ek;
??Loopstack?Ek_1;
??Loopstack?Uk;
??
??INT32S?sumi;
??INT16S?KP;
??INT16S?KI;
??INT16S?KD;?
}?PID;
?
typedef?struct?speed_pid
{
INT16S?ek;
INT16S?ek_1;
INT16S?ek_2;
INT16S?uk;

INT16S?SA;
INT16S?SB;
INT16S?SC;
}?SPEED_PID;
?SPEED_PID?speed_control_manage;

?INT16U?STORE_SPEED;
????
????
?/*
??**************************************************************
??*Description:???PID調節公式
??*Arguments:?????指向PID結構的指針
??*Returns：??????PID計算結果值
??**************************************************************
??*/
??INT16S?pid_manage（PID?*pdata）
??{
????pdata->Uk=（pdata->KP）*（pdata->Ek）+（pdata->KI）*（pdata->sumi）+（pdata->KD）*（pdata->Ek-pdata->Ek_1）;
????return?（pdata->Uk）;
??}
?
?/*
?****************************************************************
?*description:??速度PID?調節公式
?*aruments:?????指向pid結構的指針
?*returns:??????PID計算結果值
?*****************************************************************
?
?INT16S?Speed_pid_manage（PID?*pdata）
?{
???pdata->Uk+=（pdata->KP）*（pdata->Ek）+（pdata->KI）*（pdata->Ek）+（pdata->KD）*（pdata->Ek-pdata->Ek_1）;
???if（pdata->Uk>MAX_DTY）
???????pdata->Uk=?MAX_DTY;
???if（pdata->Uk<-MAX_DTY）
??????pdata->Uk=-MAX_DTY;
???return?pdata->Uk;
?}*/
??INT16S?Speed_pid_manage（SPEED_PID?*pdata）
?{
???pdata->uk+=（pdata->SA）*（pdata->ek）+（pdata->SB）*（pdata->ek_1）+（pdata->SC）*（pdata->ek_2）;
???if（pdata->uk>MAX_DTY）
???????pdata->uk=?MAX_DTY;
???if（pdata->uk<-MAX_DTY）
??????pdata->uk=-MAX_DTY;
???return?pdata->uk;
?}
/*-------------------------------------------------------------------------------------*/

/*-------------------------------------儲存緩沖區---------------------------------------------------------------------------*/


Loopstack??steer_ploop[25]=??????????????????//舵機偏差儲存隊列?構造存儲隊列25個元素?空位置為0?存儲區初始化為中間
{0000000000000000000000000};



typedef?struct?Loop???????????????????????????//數據緩沖區存儲指針結構
{?????????????????????????????????????
???Loopstack?*?startpointer;?????????????????//隊列開頭指針
???Loopstack?*?endpointer;???????????????????//隊列尾指針
???
}?Loop;
?Loop??steer_quq=??????????????????????//舵機隊列頭尾指針?存儲隊列初始化
{????????????????????????????????????????????//隊列長度20個元素
??&steer_ploop[3]&steer_ploop[22]
};

?
?????
//const?INT8S?Control_table[3][25]=
//{?
//{11	11	11	10	10	6	6	6	6	3	3	0	0	0	0	0	0	0	0	0	-6	-6	-6	-6	-6}
//{11	11	11	10	10	6	5	5	5	3	3	0	0	0	-3	-3	-4	-4	-5	-5	-10	-10	-10	-10	-10}
//?{6	6	6	5	5	3	3	3	0	0	0	0	0	0	-3	-3	-6	-6	-6	-6	-10	-10	-10	-10	-10}
??
??
//}?;
/*
**

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2012-07-24?18:30??FREESCALE?DOCUMENT2\
?????目錄???????????0??2013-04-13?22:21??FREESCALE?DOCUMENT2\Project_1\
?????目錄???????????0??2013-04-13?22:21??FREESCALE?DOCUMENT2\Project_1\bin\
?????文件??????220025??2012-04-12?23:55??FREESCALE?DOCUMENT2\Project_1\bin\Project.abs
?????文件????????7926??2012-04-12?23:55??FREESCALE?DOCUMENT2\Project_1\bin\Project.abs.phy
?????文件????????7722??2012-04-12?23:55??FREESCALE?DOCUMENT2\Project_1\bin\Project.abs.s19
?????文件??????118636??2012-04-12?23:55??FREESCALE?DOCUMENT2\Project_1\bin\Project.map
?????文件???????????0??2012-04-11?22:21??FREESCALE?DOCUMENT2\Project_1\bin\Project.xpr
?????目錄???????????0??2012-07-24?18:30??FREESCALE?DOCUMENT2\Project_1\cmd\
?????文件????????1511??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Erase_unsecure_hcs12p.cmd
?????文件??????????59??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Postload.cmd
?????文件??????????60??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Preload.cmd
?????文件??????????60??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Reset.cmd
?????文件??????????59??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Startup.cmd
?????文件??????????77??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Vppoff.cmd
?????文件??????????78??2012-03-15?21:02??FREESCALE?DOCUMENT2\Project_1\cmd\P&E_Multilink_USB_Vppon.cmd
?????文件????????1511??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Erase_unsecure_hcs12p.cmd
?????文件??????????59??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Postload.cmd
?????文件??????????60??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Preload.cmd
?????文件??????????60??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Reset.cmd
?????文件??????????59??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Startup.cmd
?????文件??????????77??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Vppoff.cmd
?????文件??????????78??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\cmd\TBDML_Vppon.cmd
?????文件?????????855??2012-03-15?00:02??FREESCALE?DOCUMENT2\Project_1\C_Layout.hwl
?????文件?????????161??2004-06-15?15:46??FREESCALE?DOCUMENT2\Project_1\Default.mem
?????目錄???????????0??2012-07-24?18:30??FREESCALE?DOCUMENT2\Project_1\DOCUMENT\
?????文件????????7812??2012-04-12?23:55??FREESCALE?DOCUMENT2\Project_1\DOCUMENT\Dataprocess.c
?????文件?????????112??2012-04-05?20:41??FREESCALE?DOCUMENT2\Project_1\DOCUMENT\Dataprocess.h
?????文件?????????352??2012-04-11?18:29??FREESCALE?DOCUMENT2\Project_1\DOCUMENT\define.h
?????文件??????????44??2012-03-15?17:19??FREESCALE?DOCUMENT2\Project_1\DOCUMENT\document.h
?????文件????????3486??2012-04-10?09:00??FREESCALE?DOCUMENT2\Project_1\DOCUMENT\Hardwareinit.c
............此處省略54個文件信息