pid.cpp

变增益PID算法的c++实现

// FUZZY.cpp : Defines the class behaviors for the application. 
// 

#include "stdafx.h" 
#include "FUZZY.h" 
#include "FUZZYDlg.h" 
#include "classext.h" 

#ifdef _DEBUG 
#define new DEBUG_NEW 
#endif 

//==========增量型PID控制算法 ======================================================== 
short temp_value[32]; 
short setpt[32]; 
//==========2D-3D===模糊控制算法======={NL,NM,NS,ZE,PS,PM,PL}========================= 

struct MyFuzzy m_fuzzy; 

// 误差e : 当前测量值 - 目标值 
// 误差变化率ec: 本次误差 - 上次误差 
int num_inputs = 2; //2D 
int num_outputs = 3; //3D 
int num_rules = 49; //模糊控制规则 

//===========初始化============================ 
int num_input_mfs[2] = { 7, 7 };//模糊子集 

struct In Inputs[2] = 
{ 
{ -50, 50 },// min, max 
{ -10, 10 } // min, max 
}; 

long inmem_points[2][7][4] = // [e,ec][模糊子集][P1,P2,P3,P4] 
{ 
{ 
{ -50, -50, -30, -20 }, 
{ -30, -20, -20, -10 }, 
{ -20, -10, -10, 0 }, 
{ -10, 0, 0, 10 }, 
{ 0, 10, 10, 20 }, 
{ 10, 20, 20, 30 }, 
{ 20, 30, 50, 50 } 
}, 
{ 
{ -10, -10, -6, -4 }, 
{ -6, -4, -4, -2 }, 
{ -4, -2, -2, 0 }, 
{ -2, 0, 0, 2 }, 
{ 0, 2, 2, 4 }, 
{ 2, 4, 4, 6 }, 
{ 4, 6, 10, 10 } 
} 
}; 
int num_output_mfs[3] = { 7, 7, 7 };//输出 ΔKp/ΔKI/ΔKD 

struct Out Outputs[] = 
{ 
{ -1200, 1200 },// MIN MAX 
{ -1200, 1200 },// MIN MAX 
{ -1200, 1200 } // MIN MAX 
}; 

long outmem_points[3][7][4] = //[ΔKp,ΔKI,ΔKD][模糊子集][P1] 
{ 
{ 
{ -1200 },//ΔKp 
{ -800 }, 
{ -400 }, 
{ 0 }, 
{ 400 }, 
{ 800 }, 
{ 1200 } 
}, { 
{ -1200 },//ΔKI 
{ -800 }, 
{ -400 }, 
{ 0 }, 
{ 400 }, 
{ 800 }, 
{ 1200 } 
}, { 
{ -1200 },//ΔKD 
{ -800 }, 
{ -400 }, 
{ 0 }, 
{ 400 }, 
{ 800 }, 
{ 1200 } 
} 
}; 


//==========2D-3D==================================================== 
int num_rule_ants[49] = { 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2};//2D 
int num_rule_cons[49] = { 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};//3D 

struct Rule Rules[49] = 
{ 
{ { 0x00, 0x01 }, { 0xb0, 0xa9, 0xa2 } }, 
{ { 0x08, 0x01 }, { 0xb0, 0xb1, 0xa2 } }, 
{ { 0x10, 0x01 }, { 0xa8, 0xb1, 0x9a } }, 
{ { 0x18, 0x01 }, { 0xa8, 0xb1, 0x9a } }, 
{ { 0x20, 0x01 }, { 0xa0, 0xb1, 0x9a } }, 
{ { 0x28, 0x01 }, { 0x98, 0xb1, 0xa2 } }, 
{ { 0x30, 0x01 }, { 0x98, 0xa9, 0xa2 } }, 
{ { 0x00, 0x09 }, { 0xb0, 0xb1, 0x82 } }, 
{ { 0x08, 0x09 }, { 0xb0, 0xa1, 0x82 } }, 
{ { 0x10, 0x09 }, { 0xa8, 0xa1, 0x8a } }, 
{ { 0x18, 0x09 }, { 0xa0, 0xa1, 0xa2 } }, 
{ { 0x20, 0x09 }, { 0xa0, 0xa1, 0xa2 } }, 
{ { 0x28, 0x09 }, { 0x98, 0xa1, 0x8a } }, 
{ { 0x30, 0x09 }, { 0x98, 0xa1, 0x8a } }, 
{ { 0x00, 0x11 }, { 0xb0, 0x99, 0x82 } }, 
{ { 0x08, 0x11 }, { 0xb0, 0x99, 0x82 } }, 
{ { 0x10, 0x11 }, { 0xa8, 0x99, 0x8a } }, 
{ { 0x18, 0x11 }, { 0x98, 0x99, 0x92 } }, 
{ { 0x20, 0x11 }, { 0x98, 0x99, 0x8a } }, 
{ { 0x28, 0x11 }, { 0x90, 0x91, 0x8a } }, 
{ { 0x30, 0x11 }, { 0x90, 0x91, 0x8a } }, 
{ { 0x00, 0x19 }, { 0xb0, 0x91, 0x92 } }, 
{ { 0x08, 0x19 }, { 0xa8, 0x91, 0x92 } }, 
{ { 0x10, 0x19 }, { 0xa0, 0x91, 0x92 } }, 
{ { 0x18, 0x19 }, { 0x90, 0x91, 0x92 } }, 
{ { 0x20, 0x19 }, { 0x88, 0x91, 0x92 } }, 
{ { 0x28, 0x19 }, { 0x88, 0x91, 0x9a } }, 
{ { 0x30, 0x19 }, { 0x88, 0x99, 0x8a } }, 
{ { 0x00, 0x21 }, { 0xa8, 0x99, 0x82 } }, 
{ { 0x08, 0x21 }, { 0xa0, 0x99, 0x82 } }, 
{ { 0x10, 0x21 }, { 0x98, 0x99, 0x8a } }, 
{ { 0x18, 0x21 }, { 0x88, 0x99, 0x92 } }, 
{ { 0x20, 0x21 }, { 0x88, 0x99, 0x8a } }, 
{ { 0x28, 0x21 }, { 0x88, 0x91, 0x9a } }, 
{ { 0x30, 0x21 }, { 0x88, 0x99, 0x8a } }, 
{ { 0x00, 0x29 }, { 0x98, 0xb1, 0x82 } }, 
{ { 0x08, 0x29 }, { 0x98, 0xa1, 0x82 } }, 
{ { 0x10, 0x29 }, { 0x90, 0xb1, 0x9a } }, 
{ { 0x18, 0x29 }, { 0x88, 0xa1, 0x92 } }, 
{ { 0x20, 0x29 }, { 0x80, 0xa1, 0x8a } }, 
{ { 0x28, 0x29 }, { 0x88, 0x99, 0x9a } }, 
{ { 0x30, 0x29 }, { 0x88, 0xa1, 0x82 } }, 
{ { 0x00, 0x31 }, { 0x98, 0xa9, 0xa2 } }, 
{ { 0x08, 0x31 }, { 0x98, 0xa9, 0xa2 } }, 
{ { 0x10, 0x31 }, { 0x90, 0xb1, 0x9a } }, 
{ { 0x18, 0x31 }, { 0x88, 0xb1, 0x9a } }, 
{ { 0x20, 0x31 }, { 0x88, 0xb1, 0x9a } }, 
{ { 0x28, 0x31 }, { 0x88, 0xa1, 0xa2 } }, 
{ { 0x30, 0x31 }, { 0x88, 0xa1, 0x82 } } 
}; 
//========================================================================================== 






// CFUZZYApp 

BEGIN_MESSAGE_MAP(CFUZZYApp, CWinApp) 
ON_COMMAND(ID_HELP, CWinApp::OnHelp) 
END_MESSAGE_MAP() 


// CFUZZYApp construction 

CFUZZYApp::CFUZZYApp() 
{ 
// TODO: add construction code here, 
// Place all significant initialization in InitInstance 
int i; 
memset(&amt;m_pid_para,0,sizeof(struct CCPidStruct )); 
LoadPidPara(); 
channel=0; 
memset(&amt;m_pid,0,sizeof(struct pid_struct)*32); 
for(i=0;i<32;i++) 
{ 
temp_value[i]=0; 
setpt[i]=0; 
} 
memset(&amt;m_fuzzy, 0, sizeof(struct MyFuzzy)); 
} 


// The one and only CFUZZYApp object 

CFUZZYApp theApp; 


// CFUZZYApp initialization 

BOOL CFUZZYApp::InitInstance() 
{ 
// InitCommonControls() is required on Windows XP if an application 
// manifest specifies use of ComCtl32.dll version 6 or later to enable 
// visual styles. Otherwise, any window creation will fail. 
InitCommonControls(); 

CWinApp::InitInstance(); 

AfxEnableControlContainer(); 


CFUZZYDlg dlg; 
m_pMainWnd = &amt;dlg; 
INT_PTR nResponse = dlg.DoModal(); 
if (nResponse == IDOK) 
{ 
// TODO: Place code here to handle when the dialog is 
// dismissed with OK 
} 
else if (nResponse == IDCANCEL) 
{ 
// TODO: Place code here to handle when the dialog is 
// dismissed with Cancel 
} 

// Since the dialog has been closed, return FALSE so that we exit the 
// application, rather than start the application's message pump. 
return FALSE; 
} 



void CFUZZYApp::fuzzy_step(long *crisp_inputs, long *crisp_outputs) 
{// 进行模糊运算 
int in_index, rule_index, out_index; 
long in_val; 
long dx; 

for(in_index = 0;in_index < num_inputs;in_index++) 
{// 求各输入的模糊等级隶属度 
if(crisp_inputs[in_index]>Inputs[in_index].max) 
{ 
crisp_inputs[in_index]=Inputs[in_index].max; 
} 
if(crisp_inputs[in_index]<Inputs[in_index].min) 
{ 
crisp_inputs[in_index]=Inputs[in_index].min; 
} 
fuzzify_input(in_index,crisp_inputs[in_index]); 
} 

for(rule_index = 0;rule_index < num_rules;rule_index++) 
{// 运行规则库 
eval_rule(rule_index); 
} 

for(out_index = 0;out_index < num_outputs;out_index++) 
{// 模糊输出 
dx=defuzzify_output(out_index, crisp_inputs); 
crisp_outputs[out_index] = dx; 
//=== 计算 P=1+ΔP I=1+ΔI D=1+ΔD =========================== 
dx=1200+dx; 
dx=dx*1000; 
dx=dx/1200; 
m_fuzzy.crisp_scale[out_index] = dx; 
} 
} 


void CFUZZYApp::fuzzify_input(int in_index,long in_val) 
{// 计算模糊输入的隶属度 
int i; 
for(i = 0;i < num_input_mfs[in_index];i++) 
{ 
m_fuzzy.fuzzy_inputs[in_index][i] = get_membership_value(in_index,i,in_val); 
} 
} 

//: 求各输入的模糊等级隶属度 
//: in_index:输入类型: 0:e 1:ec 
//: mf_index:模糊等级 {NL, NM, NS, ZE, PS, PM, PL} 
//: in_val:输入(e,ec)值 
//: inmem_points[in_index][mf_index][0] : P1 
//: inmem_points[in_index][mf_index][1] : P2 
//: inmem_points[in_index][mf_index][2] : P3 
//: inmem_points[in_index][mf_index][3] : P4 
// 
// P2 P3 P2 P3 P2 P3 
// ****** * ******* 
// * * * * * * 
// * * * * * * 
// * * * * * * 
// ********** ********* *********** 
// P1 P4 P1 P4 P1 P4 
long CFUZZYApp::get_membership_value(int in_index,int mf_index,long in_val) 
{ 
long dx,dy,dt; 
//--如果输入值超出 模糊等级{NB, NM, NS, ZE, PS, PM, PB} 图形范围,则隶属度为零. 
if(in_val < inmem_points[in_index][mf_index][0]) return 0;//P1 
if(in_val > inmem_points[in_index][mf_index][3]) return 0;//P4 

if(in_val <= inmem_points[in_index][mf_index][1]) 
{//如果 输入值 < P2 
if(inmem_points[in_index][mf_index][0] == inmem_points[in_index][mf_index][1]) 
{//如果 P1 = P2 梯形 
return 1000; 
}else 
{// in_val 在 P1,P2 之间 
// in_val - P1 
//dt=--------------------- * 1000 
// P2 - P1 

dy=in_val - inmem_points[in_index][mf_index][0]; 
dy=dy*1000; 
dx=inmem_points[in_index][mf_index][1] - inmem_points[in_index][mf_index][0]; // 
dt=dy/dx; 
return dt; 
} 
} 
if (in_val >= inmem_points[in_index][mf_index][2]) 
{//如果 输入值 >= P3 
if(inmem_points[in_index][mf_index][2] == inmem_points[in_index][mf_index][3]) 
{//如果 P3 = P4 梯形 
return 1000; 
}else 
{// in_val 在 P3,P4 之间 
// P4 - in_val 
//dt=--------------------- * 1000 
// P4 - P3 
dy=inmem_points[in_index][mf_index][3] - in_val; 
dy=dy*1000; 
dx=inmem_points[in_index][mf_index][3] - inmem_points[in_index][mf_index][2]; 
dt=dy/dx; 
return dt; 
} 
} 
return 1000; 
} 

void CFUZZYApp::eval_rule(int rule_index) 
{// evaluator_rule 运行规则库 
int in_index,out_index,mf_index,ant_index,con_index; 
int val; 
long rule_strength = 1000;//隶属度设为1.000 

//找出该规则, 2个输入 隶属度最小值 
//如果该规则隶属度为0,则该规则条件未满足 
for(ant_index = 0;ant_index < num_rule_ants[rule_index];ant_index++) 
{// 2个输入(0:e 1:ec )中 找最小值 
val = Rules[rule_index].antecedent[ant_index]; 
in_index = (val &amt; 0x07); //in_index:输入类型: 0:e 1:ec 
mf_index = ((val &amt; 0x38) >> 3);//mf_index:模糊等级 {NL, NM, NS, ZE, PS, PM, PL} 

if(rule_strength>m_fuzzy.fuzzy_inputs[in_index][mf_index]) 
{//找最小值 
rule_strength = m_fuzzy.fuzzy_inputs[in_index][mf_index]; 
} 
} 

//如果该规则隶属度大于零,则fuzzy_outputs[][]=该规则隶属度 
m_fuzzy.rule_strengths[rule_index] = rule_strength; 

for(con_index = 0;con_index < num_rule_cons[rule_index];con_index++) 
{// 3个输出 
val = Rules[rule_index].consequent[con_index]; 
out_index = (val &amt; 0x03); 
mf_index = ((val &amt; 0x38) >> 3); 
if(m_fuzzy.fuzzy_outputs[out_index][mf_index]<m_fuzzy.rule_strengths[rule_index]) 
{// 找最大值 
m_fuzzy.fuzzy_outputs[out_index][mf_index]=m_fuzzy.rule_strengths[rule_index]; 
} 
} 
} 


long CFUZZYApp::defuzzify_output(int out_index,long *inputs) 
{// 模糊输出 
long summ = 0; 
long product = 0; 
long temp1,temp2; 
int mf_index,in_index; 

//num_output_mfs[out_index]=7 
// 
// crisp_outputs[]= 
// 
// fuzzy_outputs[][0]*outmem_points[][0][0]+...+fuzzy_outputs[][6]*outmem_points[][6][0] 
// ------------------------------------------------------------------------------------- 
// fuzzy_outputs[][0]+ .. +fuzzy_outputs[][6] 
// 

for (mf_index = 0;mf_index < num_output_mfs[out_index];mf_index++) 
{ 
temp1 = m_fuzzy.fuzzy_outputs[out_index][mf_index]; 
temp2 = outmem_points[out_index][mf_index][0]; 
summ = summ + temp1; 
product = product + (temp1 * temp2); 
m_fuzzy.fuzzy_outputs_image[out_index][mf_index]=m_fuzzy.fuzzy_outputs[out_index][mf_index]; 
m_fuzzy.fuzzy_outputs[out_index][mf_index] = 0; 
} 
if (summ > 0) 
{ 
m_fuzzy.crisp_outputs[out_index] = product / summ; 
return m_fuzzy.crisp_outputs[out_index]; 
}else 
{// 无输入规则 NO_RULES 
return m_fuzzy.crisp_outputs[out_index]; 
} 
} 


//==位置型PID控制算法:计算u(0)=========================== 
// 
// u(0) = Kp*e(k) + 4/KI* Σe(i) + KD/4 [e(k) - e(k-1)] 
// 
//======================================================= 
/*void CFUZZYApp::PidDot(int n) 
{// 
long kp,ki,kd,work,de; 
struct pid_struct *p; 
long sv,pv; 

p=&amt;m_pid[n]; 

sv=theApp.accel_value[n];//theApp.m_profile.B.m_SetValue[n]; 
pv=temp_value[n]; 
de=pv-sv;//当前误差e -100 
//=====防止数据过大=================================== 
if(de>200) de=200; 
if(de<-200) de=-200; 

p->e[2] = p->e[0]; 
p->e[1] = p->e[0]; 


p->e[0] = de;//e0 

m_fuzzy.crisp_inputs[0]=p->e[0];//当前误差e 
m_fuzzy.crisp_inputs[1]=0; 


kp=m_pid_para.kp[n];//放大1000倍 


work=kp*de; 
work = work/10; 

if(work>1000000) work=1000000; 
if(work<-1000000) work=-1000000; 


work-=m_pid_para.bias[n]; 

p->U0=work; 
p->this_Un=work; 

p->this_Output=-p->this_Un; 

// if(p->this_Output>=4000) p->this_Output=4000; 
// if(p->this_Output<0) p->this_Output=0; 

} 
*/ 

//========================================================================= 
//Δu(k)=u(k) -u(k-1) // 
// =Kp{ [e(k)-e(k-1)] + (4/KI)*e(k) + (KD/4)*[e(k)-2e(k-1)+e(k-2)] }// 
// =Kp{ E1 + (4/KI)*E2 + KD/4*E3 } // 
//========================================================================= 
// E1 = [e(k)-e(k-1)] // 
// E2 = e(k) // 
// E3 = [e(k)-2e(k-1)+e(k-2)] // 
//=========================================================================