pendulum.cpp

这是一个倒单摆的模糊控制程序。用C++写的程序。

#include "pendulum.h"

void main(){

	//create output files

	FILE *IF;		//input force
	IF=fopen("inputForce.txt","wt");
	FILE *AP;
	AP=fopen("angularPosition.txt","wt");


	//initialization
	initial();

	printf("Enter g0's value\n");
	scanf("%f",&g0);
	printf("Enter g1's value\n");
	scanf("%f",&g1);
	printf("Enter h's value\n");
	scanf("%f",&hh);

	do {
		err=(0-y[0])*g0;
		err_dot=(0-y[1])*g1;
		u=fuzzy_control_coa(err,err_dot)*hh;
		fprintf(IF,"%f%10.7f\n",t,u);
		fprintf(AP,"%f%10.7f\n",t,y[0]);
		RK(y,u,h);
		t+=h;
	} while (t<tmax);
			

	fclose(IF);
	fclose(AP);

}//end main 


//functions 
//======================initial======================================
void initial() {
	n=3;
	u=0.0;
	t=0.0;
	y[0]=0.1;
	y[1]=0.0;
	y[2]=0.0;
	err=0.0,err_dot=0.0;
}

//========================FLC=======================================
double fuzzy_control_coa(double e_k,double dete_k) {
	int t=0,a=0,ek=0,dek=0;
	double d=0,y_star=0,sum1=0,sum2=0;
	double max1=0,max2=0,max3=0,max4=0,max5=0;

	//==================e============================================
	for (t=boundary1;t<boundary2;t++) {
		poss1_x1=leftTrapezoid(e_k,-3,-PI/2,-PI/4);
		poss1_x2=triangular(e_k,-PI/2,-PI/4,0);
		poss1_x3=triangular(e_k,-PI/4,0,PI/4);
		poss1_x4=triangular(e_k,0,PI/4,PI/2);
		poss1_x5=rightTrapezoid(e_k,PI/4,PI/2,3);

			//========================det_e==========================
		poss2_x1=leftTrapezoid(dete_k,-3,-PI/4,-PI/8);
		poss2_x2=triangular(dete_k,-PI/4,-PI/8,0);
		poss2_x3=triangular(dete_k,-PI/8,0,PI/8);
		poss2_x4=triangular(dete_k,0,PI/8,PI/4);
		poss2_x5=rightTrapezoid(dete_k,PI/8,PI/4,3);

	}
		//===========================================================

		poss_min[0][0]=min(poss1_x1,poss2_x1);
		poss_min[0][1]=min(poss1_x1,poss2_x2);
		poss_min[0][2]=min(poss1_x1,poss2_x3);
		poss_min[0][3]=min(poss1_x1,poss2_x4);
		poss_min[0][4]=min(poss1_x1,poss2_x5);
		
		poss_min[1][0]=min(poss1_x2,poss2_x1);
		poss_min[1][1]=min(poss1_x2,poss2_x2);
		poss_min[1][2]=min(poss1_x2,poss2_x3);
		poss_min[1][3]=min(poss1_x2,poss2_x4);
		poss_min[1][4]=min(poss1_x2,poss2_x5);
		
		poss_min[2][0]=min(poss1_x3,poss2_x1);
		poss_min[2][1]=min(poss1_x3,poss2_x2);
		poss_min[2][2]=min(poss1_x3,poss2_x3);
		poss_min[2][3]=min(poss1_x3,poss2_x4);
		poss_min[2][4]=min(poss1_x3,poss2_x5);

		poss_min[3][0]=min(poss1_x4,poss2_x1);
		poss_min[3][1]=min(poss1_x4,poss2_x2);
		poss_min[3][2]=min(poss1_x4,poss2_x3);
		poss_min[3][3]=min(poss1_x4,poss2_x4);
		poss_min[3][4]=min(poss1_x4,poss2_x5);

		poss_min[4][0]=min(poss1_x5,poss2_x1);
		poss_min[4][1]=min(poss1_x5,poss2_x2);
		poss_min[4][2]=min(poss1_x5,poss2_x3);
		poss_min[4][3]=min(poss1_x5,poss2_x4);
		poss_min[4][4]=min(poss1_x5,poss2_x5);

		//Find the largest tau value

		max1=max(poss_min[0][0],poss_min[0][1],poss_min[0][2],poss_min[1][0],poss_min[1][1],poss_min[2][0]);
		max2=max(poss_min[0][3],poss_min[1][2],poss_min[2][1],poss_min[3][0],0,0);
		max3=max(poss_min[0][4],poss_min[1][3],poss_min[2][2],poss_min[3][1],poss_min[4][0],0);
		max4=max(poss_min[1][4],poss_min[2][3],poss_min[3][2],poss_min[4][1],0,0);
		max5=max(poss_min[2][4],poss_min[3][3],poss_min[3][4],poss_min[4][2],poss_min[4][3],poss_min[4][4]);
 
		//==================det_u=====================================

		a=0;
		for (t=boundary1;t<boundary2;t++) {
			d=(double)t/10.0;
			trib_1[a]=ub_triangular(d,-30,-20,-8.6,max5);
			trib_2[a]=ub_triangular(d,-20,-8.6,0,max4);
			trib_3[a]=ub_triangular(d,-8.6,0,8.6,max3);
			trib_4[a]=ub_triangular(d,0,8.6,20,max2);
			trib_5[a]=ub_triangular(d,8.6,20,30,max1);
			a++;
		}

int kk=0;
double kkk=0;
sum1=0;
sum2=0;
for (t=boundary1;t<boundary2;t++) {
	kkk=(double)t/10.0;
	tatal[kk]=tatal_ub(trib_1[kk],trib_2[kk],trib_3[kk],trib_4[kk],trib_5[kk]);
	sum2+=(tatal[kk]*kkk); //coa defuzzify denomurator
	sum1+=tatal[kk];
	kk++;
}

//============================================================================

	if (sum1==0) y_star=0;
	else y_star=sum2/sum1;
	return y_star;
} //end coa


double leftTrapezoid(double x, double a1,double a2,double a3) {
	double ans=0;
		if (x < a1) ans = 0;
			else if ((x >= a1) && (x <=a2))  ans = 1;
				else if ((x > a2) && (x <= a3)) ans = (a3-x)/(a3-a2);
					else if (x > a3) ans = 0;
						return ans;

	
}// leftTrapezoid

double triangular(double x, double a1,double a2,double a3)
{

	double ans=0;
	if (x <= a1) ans = 0;
		else if ((x > a1) && (x <=a2))   ans = (x-a1)/(a2-a1);
			else if ((x > a2) && (x <= a3)) ans = (a3-x)/(a3-a2);
				else if (x > a3) ans = 0;
					return ans;
						
}// end triangular

double rightTrapezoid(double x, double a1,double a2,double a3)
{
		double ans=0;
		if ((x < a1) || (x > a3))
			ans = 0;
		else
			if ((x >= a1) && (x <=a2))
				ans = (x-a1)/(a2-a1);
			else 
			if ((x >= a2) && (x <= a3))
				ans = 1;
			return ans;

} // end rightTrapezoid

double min(double a,double b) 
{
	double min=0;
	if (a>b) 
		min=b;
	else 
		min=a;
	return min;
}//min

double max(double a,double b,double c,double d, double e,double f)
 {
	double ans;
	if (a>=b) 
		ans=a;
	else 
		ans=b;
	if (c>=ans) 
		ans=c;
	else 
		ans=ans;
	if (d>=ans) 
		ans=d;
	else 
		ans=ans;
	if(e>=ans) 
		ans=e;
	else 
		ans=ans;
	if (f>=ans)
		ans=f;
	else
		ans=ans;
	return ans;
}//max


//Create ub_triangular function

double ub_triangular(double x,double a1,double a2,double a3,double max) 
{
	double ans=0;
	if (x<=a1) 
		ans=0;
	else if (x>a1&&x<=a2) 
		ans=(x-a1)/(a2-a1);
	else if (x>a2&&x<=a3) 
		ans=(a3-x)/(a3-a2);
	else if (x>a3) 
		ans=0;
	if(ans>max) 
		ans=max;
	else 
		ans=ans;
	return ans;
} //end ub_triangular

//tatal
double tatal_ub(double a, double b, double c,double d,double e) 
{
	double ans;
	if (a>=b) 
		ans=a;
	else 
		ans=b;
	if (c>=ans) 
		ans=c;
	else 
		ans=ans;
	if (d>=ans) 
		ans=d;
	else 
		ans=ans;
	if (e>=ans) 
		ans=e;
	else 
		ans=ans;
	return ans;
}