#include "FuzzyPID.h"
#include "math.h"
//FuzzyTab[7]={-6,-4,-2,0,2,4,6}
#define NB (-6.0f)
#define NM (-4.0f)
#define NS (-2.0f)
#define ZO (0.0f)
#define PS (2.0f)
#define PM (4.0f)
#define PB (6.0f)
const float32 KpRuleTab[7][7]=
{{PB,PB,PM,PS,PS,ZO,ZO},
{PB,PB,PM,PS,PS,ZO,NS},
{PM,PM,PM,PS,ZO,NS,NS},
{PM,PM,PS,ZO,NS,NM,NM},
{PS,PS,ZO,NS,NS,NM,NM},
{PS,ZO,NS,NM,NM,NM,NB},
{ZO,ZO,NM,NM,NM,NB,NB}};
const float32 KiRuleTab[7][7]=
{{NB,NB,NM,NM,NS,ZO,ZO},
{NB,NB,NM,NS,NS,ZO,ZO},
{NB,NM,NS,NS,ZO,PS,PS},
{NM,NM,NS,ZO,PS,PM,PM},
{NM,NS,ZO,PS,PS,PM,PB},
{ZO,ZO,PS,PS,PM,PB,PB},
{ZO,ZO,PS,PM,PM,PB,PB}};
const float32 KdRuleTab[7][7]=
{{PS,NS,NB,NB,NB,NM,PS},
{PS,NS,NB,NM,NM,NS,ZO},
{ZO,NS,NM,NM,NS,NS,ZO},
{ZO,NS,NS,NS,NS,NS,ZO},
{ZO,ZO,ZO,ZO,ZO,ZO,ZO},
{PB,NS,PS,PS,PS,PS,PB},
{PB,PM,PM,PM,PS,PS,PB}};
void LinearCalc(ST_FUZZYPDI *p_str,float32 *linearValue)
{
float32 curntDelta,D_delta;
curntDelta = p_str->f32_refValue - p_str->f32_ActValue; //calculate Delta
D_delta = curntDelta - p_str->f32_lastDelta; //calculate d_Delta
*linearValue = 6.0f * curntDelta/(p_str->f32_maxSignal - p_str->f32_minSignal);
*(linearValue+1) = 3.0f * D_delta/(p_str->f32_maxSignal - p_str->f32_minSignal);
}
void CalcMembership(uint8 *p_index,float32 linearValue,float32 *p_memship)
{
if((linearValue>=NB)&&(linearValue<NM))
{
*p_index = 0;
*(p_index+1) = 1;
*p_memship = -0.5f*linearValue - 2.0f;
*(p_memship+1) = 0.5f*linearValue + 3.0f;
}
else if((linearValue>=NM)&&(linearValue<NS))
{
*p_index = 1;
*(p_index+1) = 2;
*p_memship = -0.5f*linearValue - 1.0f;
*(p_memship+1) = 0.5f*linearValue + 2.0f;
}
else if((linearValue>=NS)&&(linearValue<ZO))
{
*p_index = 2;
*(p_index+1) = 3;
*p_memship = -0.5f*linearValue;
*(p_memship+1)= 0.5f*linearValue + 1.0f;
}
else if((linearValue>=ZO)&&(linearValue<PS))
{
*p_index = 3;
*(p_index+1) = 4;
*p_memship = -0.5f*linearValue + 1.0f;
*(p_memship+1) = 0.5f*linearValue;
}
else if((linearValue>=PS)&&(linearValue<PM))
{
*p_index = 4;
*(p_index+1) = 5;
*p_memship = -0.5f*linearValue + 2.0f;
*(p_memship+1) = 0.5f*linearValue - 1.0f;
}
else if((linearValue>=PM)&&(linearValue<=PB))
{
*p_index = 5;
*(p_index+1) = 6;
*p_memship = -0.5f*linearValue + 3.0f;
*(p_memship+1) = 0.5f*linearValue - 2.0f;
}
else
{
}
}
void LinearRealization(ST_FUZZYPDI *p_str,float32 *value)
{
if(value[0]>p_str->f32_maxDkp)
{
value[0] = p_str->f32_maxDkp*p_str->f32_qkp;
}
else if(value[0]<p_str->f32_minDkp)
{
value[0] = p_str->f32_minDkp*p_str->f32_qkp;
}
else
{
value[0] *= p_str->f32_qkp;
}
if(value[1]>p_str->f32_maxDki)
{
value[1] = p_str->f32_maxDki*p_str->f32_qki;
}
else if(value[1]<p_str->f32_minDki)
{
value[1] = p_str->f32_minDki*p_str->f32_qki;
}
else
{
value[1] *= p_str->f32_qki;
}
if(value[2]>p_str->f32_maxDkd)
{
value[2] = p_str->f32_maxDkd*p_str->f32_qkd;
}
else if(value[2]<p_str->f32_minDkd)
{
value[2] = p_str->f32_minDkd*p_str->f32_qkd;
}
else
{
value[2] *= p_str->f32_qkd;
}
p_str->f32_kp += value[0];
p_str->f32_ki += value[1];
p_str->f32_kd += value[2];
if(p_str->f32_kp > p_str->f32_kpMax)
{
p_str->f32_kp = p_str->f32_kpMax;
}
else if(p_str->f32_kp < p_str->f32_kpMin)
{
p_str->f32_kp = p_str->f32_kpMin;
}
if(p_str->f32_ki > p_str->f32_kiMax)
{
p_str->f32_ki = p_str->f32_kiMax;
}
else if(p_str->f32_ki < p_str->f32_kiMin)
{
p_str->f32_ki = p_str->f32_kiMin;
}
if(p_str->f32_kd > p_str->f32_kdMax)
{
p_str->f32_kd = p_str->f32_kdMax;
}
else if(p_str->f32_kd < p_str->f32_kdMin)
{
p_str->f32_kd = p_str->f32_kdMin;
}
}
void FuzzyPID_ParaCalc(ST_FUZZYPDI *p_str)
{
float32 LineValue[2] = {0,0}; //Linear E and EC
uint8 E_index[2] = {0,0};
uint8 EC_index[2] = {0,0};
float32 E_memship[2] = {0,0};
float32 EC_memship[2] = {0,0};
float32 p_DeltaK[3] = {0,0,0};
LinearCalc(p_str,LineValue);
CalcMembership(E_index,LineValue[0],E_memship);
CalcMembership(EC_index,LineValue[1],EC_memship);
p_DeltaK[0] = E_memship[0]*(EC_memship[0]*KpRuleTab[E_index[0]][EC_index[0]]+EC_memship[1]*KpRuleTab[E_index[0]][EC_index[1]])+
E_memship[1]*(EC_memship[0]*KpRuleTab[E_index[1]][EC_index[0]]+EC_memship[1]*KpRuleTab[E_index[1]][EC_index[1]]);
p_DeltaK[1] = E_memship[0]*(EC_memship[0]*KiRuleTab[E_index[0]][EC_index[0]]+EC_memship[1]*KiRuleTab[E_index[0]][EC_index[1]])+
E_memship[1]*(EC_memship[0]*KiRuleTab[E_index[1]][EC_index[0]]+EC_memship[1]*KiRuleTab[E_index[1]][EC_index[1]]);
p_DeltaK[2] = E_memship[0]*(EC_memship[0]*KdRuleTab[E_index[0]][EC_index[0]]+EC_memship[1]*KdRuleTab[E_index[0]][EC_index[1]])+
E_memship[1]*(EC_memship[0]*KdRuleTab[E_index[1]][EC_index[0]]+EC_memship[1]*KdRuleTab[E_index[1]][EC_index[1]]);
LinearRealization(p_str,p_DeltaK);
}
void PIDController(ST_FUZZYPDI *p_str,float32 IntegralMax,float32 DiffMax,float32 PMax)
{
float32 Delta,d_Delta,P_out,I_out,D_out;
static float32 DeltaIntegral = 0.0f;
Delta = p_str->f32_refValue - p_str->f32_ActValue;
p_str->f32_Delta = Delta;
DeltaIntegral += Delta;
d_Delta = (Delta - p_str->f32_lastDelta)/p_str->f32_PeriodTime;
P_out = Delta * p_str->f32_kp;
I_out = DeltaIntegral * p_str->f32_ki;
D_out = d_Delta * p_str->f32_kd;
if(I_out>IntegralMax)
{
I_out = IntegralMax;
}
else if(I_out<-IntegralMax)
{
I_out = -IntegralMax;
}
if(D_out>DiffMax)
{
D_out = DiffMax;
}
else if(D_out < -DiffMax)
{
D_out = -DiffMax;
}
if(P_out>PMax)
{
P_out = PMax;
}
else if(P_out<-PMax)
{
P_out = -PMax;
}
p_str->f32_PIDOutput = P_out + I_out + D_out;
if(p_str->f32_PIDOutput > p_str->f32_saturationValue)
{
p_str->f32_PIDOutput = p_str->f32_saturationValue;
}
else if(p_str->f32_PIDOutput < -p_str->f32_saturationValue)
{
p_str->f32_PIDOutput = -p_str->f32_saturationValue;
}
p_str->f32_lastDelta = Delta;
}
评论0