智能車舵機pd運算

1、/**********************舵機增量式 PID 算法*********************** double ref = 0;//設(shè)置參數(shù) 設(shè)定值 double feb = 0;//采樣反饋 過程值int pwm_var = 0; //PID 調(diào)整量 int PWM_out = 0; //PWM 輸出量double Uo = 0;double Ek = 0; double Ei = 0; do

2、uble Ed = 0; #define Kp 8 //PID 調(diào)節(jié)的比例常數(shù) #define Ti 0.05 //PID 調(diào)節(jié)的積分常數(shù) #define Td 0.02 //PID 調(diào)節(jié)的微分時間常數(shù) #define T 0.02 //采樣周期#define Kpp Kp * ( 1 + (T / Ti) + (Td / T) )#define Ki (-Kp) * (

3、1 + (2 * Td / T ) )#define Kd Kp * Td / T//#define Kpp 4//#define Ki 0.8//#define Kd 20//誤差的閥值，小于這個數(shù)值的時候，不做 PID 調(diào)整，避免誤差較小時頻繁調(diào)節(jié) 引起震蕩 #define Emin 3//調(diào)整值限幅，防止積分飽和 #define Umax 100 #define Umin -100//輸出值限幅 #define Pmax

4、 15500#define Pmin 200 /////////////////////////////////////////////////////////////////// ////// PID 運算 ///////1#define G_Kp 8 //P 8 #define G_Ki 15 //I #define G_Kd 0 //D int error;i

5、nt pre_error; int last_error;int U_error;uint U_Pre1=9000; //擺頭舵機中心位置 uint U_Pre2=8600; //方向舵機中心位置 uchar sum;uchar temp,tt,ss;/**************************************************函數(shù)：delay_ms()描述：延遲 x*10 微秒

6、**************************************************/ void delay(uint time) {while(time--){_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);_asm(nop);} }void display(){sum = 0;error