controltable.asv

一个用Visual C语言编写的PID控制程序

#include "mex.h"

//			   -6, -5 ,-4 ,-3 ,-2 ,-1 , 0 , 1 , 2 , 3 , 4 , 5 , 6
#define NB_X  {1.0,0.8,0.7,0.4,0.1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
#define NM_X  {0.2,0.7,1.0,0.7,0.3,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
#define NS_X  {0.0,0.1,0.3,0.7,1.0,0.7,0.2,0.0,0.0,0.0,0.0,0.0,0.0}
#define NZ_X  {0.0,0.0,0.0,0.0,0.1,0.6,1.0,0.0,0.0,0.0,0.0,0.0,0.0}
#define PZ_X  {0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.6,0.1,0.0,0.0,0.0,0.0}
#define PS_X  {0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.7,1.0,0.7,0.3,0.1,0.0}
#define PM_X  {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.7,1.0,0.7,0.3}
#define PB_x  {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.1,0.4,0.7,0.8,1.0}


#define NB_YZ {1.0,0.7,0.3,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
#define NM_YZ {0.3,0.7,1.0,0.7,0.3,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
#define NS_YZ {0.0,0.0,0.3,0.7,1.0,0.7,0.3,0.0,0.0,0.0,0.0,0.0,0.0}
#define ZE_YZ {0.0,0.0,0.0,0.0,0.3,0.7,1.0,0.7,0.3,0.0,0.0,0.0,0.0}
#define PS_YZ {0.0,0.0,0.0,0.0,0.0,0.0,0.3,0.7,1.0,0.7,0.3,0.0,0.0}
#define PM_YZ {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.3,0.7,1.0,0.7,0.3}
#define PB_YZ {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.3,0.7,1.0}

float TABLEX[8][13] = {NB_X,NM_X,NS_X,NZ_X,PZ_X,PS_X,PM_X,PB_x};
float TABLEYZ[7][13] = {NB_YZ,NM_YZ,NS_YZ,ZE_YZ,PS_YZ,PM_YZ,PB_YZ};

int INDEXZ[8][7] = { 0, 0, 0, 0, 1, 3, 3,
					 0, 0, 0, 0, 1, 3, 3,
					 1, 1, 1, 1, 3, 4, 4,
					 1, 1, 2, 3, 4, 5, 5,
					 1, 1, 2, 3, 4, 5, 5,
					 2, 2, 3, 5, 5, 5, 5,
					 3, 3, 5, 6, 6, 6, 6,
					 3, 3, 5, 6, 6, 6, 6
					};


void MinMatrix(float *pA, float *pB, float *pC)
{
//	float *pA1 = pA;
//	float *pB1 = pB;
	int n,m;
	int i = 0;
	if(pA == NULL && pB == NULL)
	{
		return;
	}
	else
	{
		for(n=0; n<13; n++)
		{
			for(m=0; m<13; m++)
			{
				if(*(pA+n) < *(pB+m))
				{
					*(pC+i) = *(pA+n);
					i++;
				}
				else
				{
					*(pC+i) = *(pB+m);
					i++;
				}
			}
		}
	}
}

void ExtractMatrix(int n, float *pC, float *pR)
{
	int i;
	if(n>13)
	{
		return;
	}
	else
	{
		for(i=0; i<13; i++)
		{
			*(pC+i) = *(pR + n*13 +i);
		}
	}
}

void Intersection(float *pC, float *minC)
{
	int i = 0;
	for(; i<13; i++)
	{
		if(*(pC +i) < *(minC +i))
		{
			*(minC+i) = *(pC+i);
		}
	}
}

void Combine(float *pC, float *maxC)
{
	int i = 0;
	for(; i<13; i++)
	{
		if(*(pC +i) > *(maxC +i))
		{
			*(maxC+i) = *(pC+i);
		}
	}
}

float zCalculate(int nx, int ny)
{
	float maxC[13] = {0,0,0,0,0,0,0,0,0,0,0,0,0};
	float pR[13][13];
	float pCa[13],pCb[13];
	float *pA, *pB, *pC;
	float n = 0,m = 0;
	int i,j,index;
	for(i = 0; i<8; i++)
	{
		for(j=0; j<7; j++)
		{
			index = INDEXZ[i][j];
			pA = TABLEX[i];
			pB = TABLEYZ[j];
			pC = TABLEYZ[index];
			
			//Ca matrix
			MinMatrix(pA,pC,pR);
			ExtractMatrix(nx, pCa, pR);
//			Intersection(pCa,minC);
			
			//Cb matrix
			MinMatrix(pB,pC,pR);
			ExtractMatrix(ny, pCb, pR);

			Intersection(pCb, pCa);			//get Ca and Cb's intersection ;store in pCa

			Combine(pCa, maxC);
		}
	}
	
	
	for(i = 0,j=-6; i<13; i++,j++)
	{
		if( maxC[i] != 0)
		{
			n += maxC[i] * j;
			m += maxC[i]; 
		}
	}
	if(m != 0)
	{
		return n/m; 
	}
	else return 0;
}

/*
void main(void)
{
	int nx, ny;
	int i,j,n;
	printf(" -6  -5   -3   -2  -1   0   1   2   3   4   5   6   ");
	for(i=0,n=-6; i<13; i++, n++)
	{
		printf("\n");
//		printf("%d",n);
		for(j=0; j<13; j++)
		{
			printf(" %f",zCalculate(i,j));
		}
	}

}*/
void CopyMatrix(double * Out ,int M, int N)
{
    int i,j;
    for(i=0; i<M; i++)
	{
		for (j =0; j<N; j++)
		{
			Out[M*i + j] = zCalculate(i,j);
		}
	}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	mxArray *In_ptr, *Out_ptr;
	double *In, *Out;
	int M,N;
//	int i,j;
	//check parameter number and type
/*	In_ptr=prhs[0];
	if(nrhs != 1)
	{
		mexErrMsgTxt("nly one input arguement allowed!");
	}
	else if (nlhs != 1)
	{
		mexErrMsgTxt("Only one output arguement allowed!");
	}
	if (!mxIsNumeric(In_ptr) || mxIsComplex(In_ptr)) || mxIsSparse(In_ptr) || !mxIsDouble(In_ptr))
	{
		mexErrMsgTxt("Input arguement muxt be a full floating point matrix!");
	}*/

	//create a new matrix
//	M = mxGetM(In_ptr);
//	N = mxGetN(In_ptr);
	M = 8;
	N = 7;
	Out_ptr = mxCreateDoubleMatrix(M,N,mxREAL);
	Out = mxGetPr(Out_ptr);
	
    CopyMatrix(Out, M, N);

}