#include?“include.h“
#include??
/////////////////////////////////////////////////////////////////////////////////start變量定義/////////////////////////////////////////////////////////////////////////////////

extern?float?DirectionOutput;
int???PD_m_addMoter_out1Moter_out2?;?//PID?的增量輸出
extern?char?error;
float?speedout;

//////////////////////////////////////////////////////////////////////////////////end?變量定義////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////start?PID?函數的編寫////////////////////////////////////////////////////////////////////////

void?Delay_us（unsigned?int?t）?
{
????while（t--）;???
}

void?stand_control（）????????
{

float?sumadd_angle;




speedout=（PID_new-PID_old）*num/8+PID_old;


add_angle=0;?

current_error=real_angle+add_angle;

sum=Gyr_jiao*PD_M_Kd;

if（sum>=500）
??sum=500;
if（sum<=-500）
sum=-500;
???
?if（current_error>0）?????//角度大于目標值
?{
?
?PD=current_error*PD_M_Kp+sum;?
?
?PD_m_add=PD;?//電機的?PID?增量值輸出
?
?if（PD_m_add>=1800）{PD_m_add=1800;PD=1800;}?//限制電機的最大速度

?
???qian=0;
???hou=1;

?}?
?else?if（current_error<0）??????//角度小于目標值
?{
?PD=-current_error*PD_M_Kp-sum;?
?PD_m_add=PD;?//電機的?PID?增量值輸出
?if（PD_m_add>=1800）{PD_m_add=1800;PD=1800;}?//限制電機的最大速度

??qian=1;
???hou=0;
?}

???if（qian==1）
??fangxiang（1001）;??
??if（hou==1）
??????
??fangxiang（0110）;?
??
speedout=0;
DirectionOutput=0;
Moter_out2=PD_m_add-speedout+DirectionOutput;?
Moter_out1=PD_m_add-speedout-DirectionOutput;
?if（Moter_out1>1700）Moter_out1=1700;
?if（Moter_out1<-300）?Moter_out1=-300;
??if（Moter_out2>1700）Moter_out2=1700;
?if（Moter_out2<-300）?Moter_out2=-300;
?PWMDTY01=Moter_out1+300;????//?（右輪）
?PWMDTY23=Moter_out2+300;????//?（左輪）//跑的方向?????????


}

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2015-05-12?07:25??飛思卡爾\
?????文件?????????855??2015-04-12?23:44??飛思卡爾\C_Layout.hwl
?????文件?????????845??2015-04-19?11:18??飛思卡爾\Datehandle.c
?????文件?????????632??2015-04-12?23:44??飛思卡爾\Datehandle.h
?????文件?????????161??2015-04-12?23:44??飛思卡爾\Default.mem
?????文件????????3524??2015-05-08?16:43??飛思卡爾\DirectionControl.c
?????文件?????????162??2015-05-08?10:53??飛思卡爾\DirectionControl.h
?????目錄???????????0??2015-05-12?07:25??飛思卡爾\Sources\
?????目錄???????????0??2015-05-12?07:25??飛思卡爾\Sources\CODE\
?????文件????????2582??2015-04-12?23:44??飛思卡爾\Sources\CODE\ADC.c
?????文件????????1348??2015-04-12?23:44??飛思卡爾\Sources\CODE\ADC.h
?????文件????????5832??2015-04-12?23:44??飛思卡爾\Sources\CODE\CPU.c
?????文件????????1686??2015-04-12?23:44??飛思卡爾\Sources\CODE\CPU.h
?????文件????????3548??2015-05-10?08:55??飛思卡爾\Sources\CODE\Events.c
?????文件????????1698??2015-04-12?23:44??飛思卡爾\Sources\CODE\Events.h
?????文件????????1759??2015-04-12?23:44??飛思卡爾\Sources\CODE\PIT.c
?????文件????????1321??2015-04-12?23:44??飛思卡爾\Sources\CODE\PIT.h
?????文件????????4367??2015-04-12?23:44??飛思卡爾\Sources\CODE\SCI.c
?????文件????????3645??2015-04-12?23:44??飛思卡爾\Sources\CODE\SCI.h
?????文件???????11560??2015-04-12?23:44??飛思卡爾\Sources\CODE\Vector.c
?????文件????????1346??2015-04-12?23:44??飛思卡爾\Sources\CODE\Vector.h
?????文件???????23855??2015-04-12?23:44??飛思卡爾\Sources\Start12.c
?????文件???????69907??2015-04-12?23:44??飛思卡爾\Sources\datapage.c
?????文件?????????262??2015-04-12?23:44??飛思卡爾\Sources\derivative.h
?????文件????????2644??2015-05-11?00:46??飛思卡爾\Sources\main.c
?????文件???????26259??2015-05-08?14:09??飛思卡爾\Sources\user.c
?????文件????????2567??2015-04-29?09:19??飛思卡爾\Sources\user.h
?????文件????????3120??2015-05-11?00:52??飛思卡爾\TBDML.ini
?????目錄???????????0??2017-12-16?10:04??飛思卡爾\U抃Metrowerks\
?????目錄???????????0??2015-05-12?07:25??飛思卡爾\bin\
?????文件??????312906??2015-05-11?00:51??飛思卡爾\bin\Project.abs
............此處省略54個文件信息