fuzzy.cpp

模糊PID控制器

		{// 找最大值
			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=&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)]											//
//=========================================================================
//误差e :			当前测量值  -  目标值								//
//误差变化率ec:     本次误差    -  上次误差								//
//=========================================================================
//PID周期为4秒=4000ms													//
//																		//
//																		//
//=========================================================================
/*void CFUZZYApp::PidRun(int n)
{// 
	long kp,ki,kd,work,wtmp,sum,de,scale,kp0,ki0,kd0,integral;
	struct pid_struct *p;
	long sv,pv;	

	p=&m_pid[n];

	sv=setpt[n];
	pv=temp_value[n];	
	integral = theApp.m_pid_para.integral[n];// 放大10倍//开始调节   放大10倍	
	sum=sv-pv;

    if(sum>=0)
    {	
		if(sum>=integral)
		{//未到开始调节下限	
			p->this_Output=4000;
			theApp.accel_value[n]=pv;
			p->this_Un=0;
			p->prev_Un=0;
			return;
		}
	}else
	{
		sum=-sum;
		if(sum>=integral)
		{// 超过开始调节上限
			p->this_Output=0;
			theApp.accel_value[n]=pv;
			p->this_Un=0;
			p->prev_Un=0;			
			return;
		}		
	}
    
    theApp.accel_value[n]+=theApp.m_pid_para.accel[n];//加速速率   放大10倍	    
	if(theApp.accel_value[n]>sv)
	{
		theApp.accel_value[n]=sv;
	}

	if(p->first==0)
	{//计算U0
		   theApp.accel_value[n]=pv;
           PidDot(n);
		   p->first=0xff;
	}else
	{
			p->prev_Un=p->this_Un;//保存上次Un记录
			p->e[2] = p->e[1];
			p->e[1] = p->e[0];

			sv=theApp.accel_value[n];//theApp.m_profile.B.m_SetValue[n];			
			pv=temp_value[n];	

			de=pv-sv;//当前误差e
			//=====防止数据过大===================================
			if(de>200)  de=200;
			if(de<-200) de=-200;
			p->e[0] = de;
			p->E1   = p->e[0]- p->e[1];//当前误差变化率ec
			p->E2   = p->e[0];
			p->E3   = p->e[0]- 2*p->e[1]+p->e[2];
				
			//====模糊推理==================================
			m_fuzzy.crisp_inputs[0]=p->e[0];//当前误差e
			work=p->E1;  //当前误差变化率ec  加权
			wtmp=m_fuzzy.crisp_inputs[1];
			sum=wtmp+work;
			sum=sum/2;
			m_fuzzy.crisp_inputs[1]=sum;
			fuzzy_step(m_fuzzy.crisp_inputs,m_fuzzy.crisp_outputs);
		        
			//====PID运算=============================================
			kp0=m_pid_para.kp[n];//放大1000 000倍     2000 * 1000
			ki0=m_pid_para.ki[n];//放大10000倍        1666 * 100
			kd0=m_pid_para.kd[n];//放大10000倍        1333 * 100
//          if(ki0>0)
//			{
//				ki0=400/ki0;
//			}else  ki0=0;

			kp=m_fuzzy.crisp_scale[0]*kp0;	
			ki=m_fuzzy.crisp_scale[1]*ki0;	
			kd=m_fuzzy.crisp_scale[2]*kd0;	
				
			kp=kp/1000; //放大1000倍	
			ki=ki/1000;	//放大10倍		
			kd=kd/100;	//放大10倍		

			//------dUn=Kp{ E1 + KI*E2 + KD*E3 }----
			work = kp*p->E1 + ki*p->E2 + kd*p->E3; //-1333
			work=work/10;

			//========对this_Un进行加权处理，提高抗干扰能力=======================
			if(work>1000000)   work=1000000;
			if(work<-1000000)  work=-1000000;
			sum=work;
			//sum=work+p->dUn;
			//sum=sum/2;
			p->dUn=sum;
			
			p->this_Un=p->prev_Un+p->dUn;
			p->this_Output=-p->this_Un;

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

	}
}
*/




void CFUZZYApp::PidRun(int n)
{// 
	long kp,ki,kd,work,wtmp,sum,de,scale,kp0,ki0,kd0,integral;
	struct pid_struct *p;
	long sv,pv;	

	p=&m_pid[n];

	sv=setpt[n];
	pv=temp_value[n];	
	integral = theApp.m_pid_para.integral[n];// 放大10倍//开始调节   放大10倍	
	sum=sv-pv;

    if(sum>=0)
    {	
		if(sum>=integral)
		{//未到开始调节下限	
			p->this_Output=4000;
			theApp.accel_value[n]=pv;
			p->this_Un=0;
			p->prev_Un=0;
			return;
		}
	}else
	{
		sum=-sum;
		if(sum>=integral)
		{// 超过开始调节上限
			p->this_Output=0;
			theApp.accel_value[n]=pv;
			p->this_Un=0;
			p->prev_Un=0;			
			return;
		}		
	}
    
	if(p->first==0)
	{//计算U0
		   theApp.accel_value[n]=pv;
		   p->first=0xff;
		   p->integral=0;
		   m_fuzzy.crisp_inputs[1]=0;
	}


    theApp.accel_value[n]+=theApp.m_pid_para.accel[n];//加速速率   放大10倍	    
	if(theApp.accel_value[n]>sv)
	{
		theApp.accel_value[n]=sv;
	}

	sv=theApp.accel_value[n];//theApp.m_profile.B.m_SetValue[n];			

	de=pv-sv;//当前误差e
	//=====防止数据过大===================================
	if(de>200)  de=200;
	if(de<-200) de=-200;
	p->e[1] = p->e[0];
	p->e[2] = p->e[1];
	p->e[0] = de;
	p->E1   = p->e[0]- p->e[1];//当前误差变化率ec


	//====模糊推理==================================
	m_fuzzy.crisp_inputs[0]=p->e[0];  //当前误差e
	m_fuzzy.crisp_inputs[1]=p->E1;    //当前误差变化率ec 
	fuzzy_step(m_fuzzy.crisp_inputs,m_fuzzy.crisp_outputs);


	//====PID运算=============================================
	kp0=m_pid_para.kp[n];//放大1000 000倍     2000 * 1000
	ki0=m_pid_para.ki[n];//放大10000倍        1666 * 100
	kd0=m_pid_para.kd[n];//放大10000倍        1333 * 100

	kp=m_fuzzy.crisp_scale[0]*kp0;	
	ki=m_fuzzy.crisp_scale[1]*ki0;	
	kd=m_fuzzy.crisp_scale[2]*kd0;	
				
	kp=kp/1000; //放大1000倍	
	ki=ki/1000;	//放大10倍		
	kd=kd/100;	//放大10倍		

	
	//=======限制积分项=======================
	p->integral=p->integral+de;
	if(p->integral>2000)  p->integral = 2000;
	if(p->integral<-2000) p->integral =-2000;

	work = kp*de + ki*p->integral+  kd*(p->e[1]-p->e[2]);

	//p->test_kd= -(kd*(p->e[1]-p->e[2]))/10;

	work=work/10;
	work-=m_pid_para.bias[n];
	work=-work;

	//========对this_Un进行加权处理，提高抗干扰能力=======================
	if(work>4000)   work=4000;
	if(work<-4000)  work=-4000;
	p->this_Output=work;
}


void CFUZZYApp::SavePidPara()
{//  
     FILE *fp;
	 CString filename;

	 filename="pid.dat";
	 fp=fopen(filename,"wb");
	 fwrite((struct CCPidStruct  *)&(m_pid_para),sizeof(struct CCPidStruct),1,fp);
     fclose(fp);
}


void CFUZZYApp::LoadPidPara()
{//  
    FILE *fp;
	CString filename;
	filename="pid.dat";
	
	fp=fopen(filename,"rb");
	if(fp==NULL)
	{
		SavePidPara();
	}else
	{
		fread((struct CCPidStruct  *)&(m_pid_para),sizeof(struct CCPidStruct),1,fp);
	    fclose(fp);
	}
}