1、1/*舵机增量式 PID 算法* double ref = 0;/设置参数 设定值double feb = 0;/采样反馈 过程值int pwm_var = 0; /PID 调整量int PWM_out = 0; /PWM 输出量double Uo = 0;double Ek = 0;double Ei = 0;double Ed = 0; #define Kp 8 /PID 调节的比例常数#define Ti 0.05 /PID 调节的积分常数#define Td 0.02 /PID 调节的微分时间常数#define T 0.02 /采样周期#define Kpp Kp * ( 1 + (T
2、 / Ti) + (Td / T) )#define Ki (-Kp) * ( 1 + (2 * Td / T ) )#define Kd Kp * Td / T/#define Kpp 4/#define Ki 0.8/#define Kd 20/误差的阀值，小于这个数值的时候，不做 PID 调整，避免误差较小时频繁调节引起震荡#define Emin 3/调整值限幅，防止积分饱和#define Umax 100#define Umin -100/输出值限幅#define Pmax 15500#define Pmin 200/ PID 运算 /2void pid_ctrl(void)Ek =
3、 ref - feb; /差值运算if( fabs(Ek) = Umax)pwm_var = Umax; /调整值限幅，防止积分饱和if(pwm_var Pmax)PWM_out = Pmax; /输出值限幅if(PWM_out 0)else if(error6)U_error = 320;else if(error8)U_error = 420;else if(error420)U_error = 420;else if(U_error 12000)U_Pre1 = 12000;else if(U_Pre1 9000) U_Pre2 = (8600+(U_Pre1-9000)*9)/10);else U_Pre2 = (8600-(9000-U_Pre1)*7)/10);if(U_Pre2 9800) U_Pre2 = 9800;else if(U_Pre2 9600)|(U_Pre29400)|(U_Pre29200)|(U_Pre29000)|(U_Pre28700)|(U_Pre28500) /直道 if(temp = 6)tt+;if(tt = 2)PWMDTY3 = 85;elsett = 0;temp = 6; /*/
