fuzzy.cpp

模糊PID控制器

// 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(&m_pid_para,0,sizeof(struct CCPidStruct ));
    LoadPidPara();
	channel=0;
	memset(&m_pid,0,sizeof(struct pid_struct)*32);
	for(i=0;i<32;i++)
	{
		temp_value[i]=0;
		setpt[i]=0;
	}
    memset(&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 = &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 & 0x07);       //in_index:输入类型:  0:e   1:ec
		mf_index = ((val & 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 & 0x03);
		mf_index = ((val & 0x38) >> 3);
		if(m_fuzzy.fuzzy_outputs[out_index][mf_index]<m_fuzzy.rule_strengths[rule_index])