模糊PID算法C语言源代码

#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; }