#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
