模糊pid控制C程序

#include "config.h" #if fuzzy_EN>0 extern fp32 angle_down_max,angle_up_max; //extern fp32 motor_xtvelocity_max; #define IS_Kp 0.3 #define IS_Ki 0.6 #define IS_Kd 0.9 #define NL -0.9 #define NM -0.6 #define NS -0.3 #define ZE 0 #define PS 0.3 #define PM 0.6 #define PL 0.9 static const fp32 fuzzyRuleKp[7][7]={ PL, PL, PM, PM, PS, PS, ZE, PL, PL, PM, PM, PS, ZE, ZE, PM, PM, PM, PS, ZE, NS, NM, PM, PS, PS, ZE, NS, NM, NM, PS, PS, ZE, NS, NS, NM, NM, ZE, ZE, NS, NM, NM, NM, NL, ZE, NS, NS, NM, NM, NL, NL }; static const fp32 fuzzyRuleKi[7][7]={ NL, NL, NL, NM, NM, ZE, ZE, NL, NL, NM, NM, NS, ZE, ZE, NM, NM, NS, NS, ZE, PS, PS, NM, NS, NS, ZE, PS, PS, PM, NS, NS, ZE, PS, PS, PM, PM, ZE, ZE, PS, PM, PM, PL, PL, ZE, ZE, PS, PM, PL, PL, PL }; static const fp32 fuzzyRuleKd[7][7]={ PS, PS, ZE, ZE, ZE, PL, PL, NS, NS, NS, NS, ZE, NS, PM, NL, NL, NM, NS, ZE, PS, PM, NL, NM, NM, NS, ZE, PS, PM, NL, NM, NS, NS, ZE, PS, PS, NM, NS, NS, NS, ZE, PS, PS, PS, ZE, ZE, ZE, ZE, PL, PL }; typedef struct{ fp32 Kp; fp32 Ki; fp32 Kd; }PID; PID fuzzy(fp32 e,fp32 ec)//误差，误差变化率 { char pe,pec; fp32 eFuzzy[2]={0,0}; fp32 ecFuzzy[2]={0,0}; static fp32 E_TABLE[]={NL,NM,NS,ZE,PS,PM,PL}; static fp32 EC_TABLE[]={NL,NM,NS,ZE,PS,PM,PL}; PID fuzzy_PID; //误差隶属函数 if(e<E_TABLE[0]) { eFuzzy[0]=1.0; pe=0; } else if(e>=E_TABLE[0]&&e<E_TABLE[1]) { eFuzzy[0]=(E_TABLE[1]-e)/(E_TABLE[1]-E_TABLE[0]); pe=0; } else if(e>=E_TABLE[1]&&e<E_TABLE[2]) { eFuzzy[0]=(E_TABLE[2]-e)/(E_TABLE[2]-E_TABLE[1]); pe=1; } else if(e>=E_TABLE[2]&&e<E_TABLE[3]) { eFuzzy[0]=(E_TABLE[3]-e)/(E_TABLE[3]-E_TABLE[2]); pe=2; } else if(e>=E_TABLE[3]&&e<E_TABLE[4]) { eFuzzy[0]=(E_TABLE[4]-e)/(E_TABLE[4]-E_TABLE[3]); pe=3; } else if(e>=E_TABLE[4]&&e<E_TABLE[5]) { eFuzzy[0]=(E_TABLE[5]-e)/(E_TABLE[5]-E_TABLE[4]); pe=4; } else if(e>=E_TABLE[5]&&e<E_TABLE[6]) { eFuzzy[0]=(E_TABLE[6]-e)/(E_TABLE[6]-E_TABLE[5]); pe=5; } else { eFuzzy[0]=0; pe=6; } eFuzzy[1]=1-eFuzzy[0]; //误差变化率隶属函数 if(e<EC_TABLE[0]) { ecFuzzy[0]=1.0; pec=0; } else if(ec>=EC_TABLE[0]&&ec<EC_TABLE[1]) { ecFuzzy[0]=(EC_TABLE[1]-ec)/(EC_TABLE[1]-EC_TABLE[0]); pec=0; } else if(ec>=EC_TABLE[1]&&ec<EC_TABLE[2]) { ecFuzzy[0]=(EC_TABLE[2]-e)/(EC_TABLE[2]-EC_TABLE[1]); pec=1; } else if(ec>=EC_TABLE[2]&&ec<EC_TABLE[3]) { ecFuzzy[0]=(EC_TABLE[3]-e)/(EC_TABLE[3]-EC_TABLE[2]); pec=2; } else if(ec>=EC_TABLE[3]&&ec<EC_TABLE[4]) { ecFuzzy[0]=(EC_TABLE[4]-e)/(EC_TABLE[4]-EC_TABLE[3]); pec=3; } else if(ec>=EC_TABLE[4]&&ec<EC_TABLE[5]) { ecFuzzy[0]=(EC_TABLE[5]-e)/(EC_TABLE[5]-EC_TABLE[4]); pec=4; } else if(ec>=EC_TABLE[5]&&ec<EC_TABLE[6]) { ecFuzzy[0]=(EC_TABLE[6]-e)/(EC_TABLE[6]-EC_TABLE[5]); pec=5; } else { ecFuzzy[0]=0; pec=6; } ecFuzzy[1]=1-ecFuzzy[0]; /*************************************反模糊*************************************/ fuzzy_PID.Kp = (eFuzzy[0] * ecFuzzy[0] * fuzzyRuleKp[pe][pec] + eFuzzy[0]*ecFuzzy[1] * fuzzyRuleKp[pe][pec+1] +eFuzzy[1]*ecFuzzy[0] * fuzzyRuleKp[pe+1][pec] + eFuzzy[1]*ecFuzzy[1]* fuzzyRuleKp[pe+1][pec+1]); fuzzy_PID.Ki = (eFuzzy[0] * ecFuzzy[0] * fuzzyRuleKi[pe][pec] + eFuzzy[0]*ecFuzzy[1] * fuzzyRuleKi[pe][pec+1] +eFuzzy[1]*ecFuzzy[0] * fuzzyRuleKi[pe+1][pec] + eFuzzy[1]*ecFuzzy[1]* fuzzyRuleKi[pe+1][pec+1]); fuzzy_PID.Kd = (eFuzzy[0] * ecFuzzy[0] * fuzzyRuleKd[pe][pec] + eFuzzy[0]*ecFuzzy[1] * fuzzyRuleKd[pe][pec+1] +eFuzzy[1]*ecFuzzy[0] * fuzzyRuleKd[pe+1][pec] + eFuzzy[1]*ecFuzzy[1]* fuzzyRuleKd[pe+1][pec+1]); return fuzzy_PID; } /********************************************************************************************************* ** 增量pid_ds_R ********************************************************************************************************/ fp32 speed_pid(fp32 tar,fp32 cur) { //float tar = 0,cur = 0; //目标值 , 实际值 static PID pid= {-15,-0.7,0}; //赋予初值kp，ki，kd fp32 lastE ; //累加偏差 fp32 e ,ec,d_u,u,Last_u,PreE; PID OUT = {0, 0, 0}; e = tar - cur; //目标值 - 实际值(e-2ec+PreE) ec = e - lastE; //误差变化率 OUT = fuzzy(e, ec); //模糊控制调整 kp，ki，kd d_u=(pid.Kp+OUT.Kp)*ec + (pid.Ki+OUT.Ki)*e + (pid.Kd+OUT.Kd)*(e-2*ec+PreE); u=d_u+Last_u; if(u>angle_up_max) u=angle_up_max; if(u<(0-angle_up_max)) u=(0-angle_up_max); PreE=lastE; lastE = e; Last_u=u; return u; } #endif