main.c

#include "bsp.h"			
#include "filtfilt.h"
#include "hilbert.h"
#include "math.h"

void LCD_ShowNum(unsigned int Num,unsigned char Length,uint16_t _usX, uint16_t _usY, FONT_T *_tFont);


void LCD_ShowNum(unsigned int Num,unsigned char Length,uint16_t _usX, uint16_t _usY, FONT_T *_tFont)
{
	char i,k,font=8;
	int j=0;
	if (Length!=0)
	{
		j=pow(10,Length-1);
		for(i=0;i<Length;i++)
		{
			k=Num/j;
			Num=Num-j*k;
			if (i!=0 || k!=0)
			{
				switch (k)
				{
					case 0:LCD_DispStr(_usX+font*i,_usY, "0", _tFont); break;
					case 1:LCD_DispStr(_usX+font*i,_usY, "1", _tFont); break;
					case 2:LCD_DispStr(_usX+font*i,_usY, "2", _tFont); break;
					case 3:LCD_DispStr(_usX+font*i,_usY, "3", _tFont); break;
					case 4:LCD_DispStr(_usX+font*i,_usY, "4", _tFont); break;
					case 5:LCD_DispStr(_usX+font*i,_usY, "5", _tFont); break;
					case 6:LCD_DispStr(_usX+font*i,_usY, "6", _tFont); break;
					case 7:LCD_DispStr(_usX+font*i,_usY, "7", _tFont); break;
					case 8:LCD_DispStr(_usX+font*i,_usY, "8", _tFont); break;
					case 9:LCD_DispStr(_usX+font*i,_usY, "9", _tFont); break;
				}
			}
			j/=10;
		}
	}
	else switch (Num)
			{
				case 0:LCD_DispStr(_usX,_usY, "0", _tFont); break;
				case 1:LCD_DispStr(_usX,_usY, "1", _tFont); break;
				case 2:LCD_DispStr(_usX,_usY, "2", _tFont); break;
				case 3:LCD_DispStr(_usX,_usY, "3", _tFont); break;
				case 4:LCD_DispStr(_usX,_usY, "4", _tFont); break;
				case 5:LCD_DispStr(_usX,_usY, "5", _tFont); break;
				case 6:LCD_DispStr(_usX,_usY, "6", _tFont); break;
				case 7:LCD_DispStr(_usX,_usY, "7", _tFont); break;
				case 8:LCD_DispStr(_usX,_usY, "8", _tFont); break;
				case 9:LCD_DispStr(_usX,_usY, "9", _tFont); break;
				case 10:LCD_DispStr(_usX,_usY, "10", _tFont); break;
				case 11:LCD_DispStr(_usX,_usY, "11", _tFont); break;
				case 12:LCD_DispStr(_usX,_usY, "12", _tFont); break;
				case 13:LCD_DispStr(_usX,_usY, "13", _tFont); break;
				case 14:LCD_DispStr(_usX,_usY, "14", _tFont); break;
				case 15:LCD_DispStr(_usX,_usY, "15", _tFont); break;
				case 16:LCD_DispStr(_usX,_usY, "16", _tFont); break;
				case 17:LCD_DispStr(_usX,_usY, "17", _tFont); break;
				case 18:LCD_DispStr(_usX,_usY, "18", _tFont); break;
				case 19:LCD_DispStr(_usX,_usY, "19", _tFont); break;
				case 20:LCD_DispStr(_usX,_usY, "20", _tFont); break;
			}
	
}


int main(void)
{
	FONT_T tFont12,tFont16;		
	char power,symbol=0;
	double S2[5120]={0};
	double S4[5120]={0};
	double x[4096];
	double y[5120];
	double a[5],b[5],adv=0,fs,max_f=0,max_ft,max_x,max_y;
	int i,n,k,time;
	bsp_Init();	
	
	
	{
		tFont12.FontCode = FC_ST_12;	    
		tFont12.FrontColor = CL_BLACK;
		tFont12.BackColor = CL_WHITE;
		tFont12.Space = 0;		
	}
	{
		tFont16.FontCode = FC_ST_16;	  
		tFont16.FrontColor = CL_BLACK;		
		tFont16.BackColor = CL_WHITE;	   
		tFont16.Space = 0;			
	}
	
	

	bsp_DelayMS(200); 
	
	LCD_ClrScr(CL_WHITE);
	bsp_DelayMS(100); 
	LCD_SetBackLight(BRIGHT_DEFAULT);	
	
	
	/* Set the parameters of the second-order Butterworth filter, which are generated by MATLAB */
	a[0]=1.0;
	a[1]=-3.82342649488893;
	a[2]=5.48479166775113;
	a[3]=-3.49929102591751;
	a[4]=0.837925858776767;
	b[0] =0.00358705477558659;
	b[1] = 0.0;
	b[2] = -0.00717410955117317;
	b[3]=0.0;
	b[4]=0.00358705477558659;
	
	
	while (1)
	{
		time = bsp_GetRunTime();
		fs=0;
		max_f=0;
		max_ft=0;
		max_x=0;
		max_y=0;
		power=0;
		symbol=0;

		LCD_DrawLine(0, 460, 800, 460, CL_BLACK);
		LCD_DrawLine(0, 210, 800, 210, CL_BLACK);
		LCD_DrawLine(80, 0, 80, 210, CL_BLACK);
		LCD_DrawLine(80, 250, 80, 460, CL_BLACK);
		for(n=1;n<20;n++)
		{

			if(n<=8)
			{
				LCD_DrawLine(80+(int)(n*87.9), 210, 80+(int)(n*87.9),205, CL_BLACK);
				LCD_ShowNum(n*500,4,80+(int)(n*87.9)-16,215,&tFont16);
			}

			LCD_DrawLine(80+n*36, 460, 80+n*36, 455, CL_BLACK);
			if(n%5==0 && n!=0)
			{
				LCD_DrawLine(80+n*36, 460, 80+n*36, 450, CL_BLACK);
				LCD_DrawLine(80+n*36+1, 460, 80+n*36+1,450, CL_BLACK);
				
			}
			LCD_ShowNum(n,0,80+n*36-5,465,&tFont16);
		}
			/* Use analog-to-digital converter to collect vibration signal and magnetic field signal during motor operation    */

		AD7606_StartConvst(); 
		for(i=0;i<10;i++)
		{
			AD7606_ReadNowAdc();
			S2[i]=g_tAD7606.sNowAdc[0];
			S4[i]=g_tAD7606.sNowAdc[1];
			bsp_DelayUS(100);
			AD7606_StartConvst();
		}

		AD7606_StartConvst();
		for(i=0;i<5120;i++)
		{
			AD7606_ReadNowAdc();
			S2[i]=g_tAD7606.sNowAdc[0];
			S4[i]=g_tAD7606.sNowAdc[1];
			bsp_DelayUS(100);
			if(i==5119)
				break;
			AD7606_StartConvst();
		}

		for(i=0;i<5120;i++)
		{
			S2[i]=S2[i]/32768.0*5.0;
			S4[i]=S4[i]/32768.0*5.0;
		}
		/* DC component of filtered magnetic signal*/
		adv=0.0;
		for(i=0;i<5120;i++)
		{
			adv=adv+S4[i];
		}
		adv=adv/5120.0;
		for(i=0;i<5120;i++)
		{
			S4[i]=S4[i]-adv;
		}
		adv=0;
		/* Filtering magnetic signal with zero phase filter */
		filtfilt(S4,5120,a,b,5,y);
		/* Cut off the first and last parts of the signal to reduce the error */
		for(i=0;i<4096;i++)
		{
			x[i]=S4[i+512];
		}
		adv=0;
		for(i=0;i<4096;i++)
		{
			adv=adv+x[i];
		}
		adv=adv/4096.0;
		for(i=0;i<4096;i++)
		{
			x[i]=x[i]-adv;
		}
		/* Using Hilbert Transform to Calculate Phase Angle of Magnetic Signal */
		get_hilbert_angle(4096,x);
		/* Unwrap the phase angle to obtain the angle signal during motor operation*/
		unwrap(4096,x);
		for(i=0;i<4096;i++)
		{
			x[i]=x[i]/3.1415926535*180;
		}

		for(i=1;i<4096;i++)
		{
			x[i]=x[i]-x[0];
		}
		x[0]=0;
		/* Obtain the frequency after resampling */
		fs=x[4095]/720.0;
		fs=4096.0/fs;
		for(i=1;i<5120;i++)
		{
			S2[i]=S2[i]-S2[0];
		}
		S2[0]=0;
		/* Resampling vibration signals using angle signals */
		resample(4096,S2,x,S4);

		/* Envelope demodulation of resampling results */
		abs_hilbert(4096,x);
		max_x=x[0];
		for(i=0;i<4096;i++)
		{
			if(x[i]>max_x)
			{
				max_x=x[i];
			}
		}

		if(max_x > 10)
		{
			symbol=1;
		}
		else if(max_x<1)
		{
			symbol=2;
		}
		n=0;
		if(symbol == 1)
		{
			while(max_x>10)
			{
				max_x/=10;
				n++;
			}
		}
		if(symbol==2)
		{
			while(max_x<1)
			{
				max_x*=10;
				n--;
			}
		}
		max_y=(int)(max_x);
		if (max_y<9)
			max_y+=1;
		for(i=1;i<=max_y;i++)
		{
			LCD_DrawLine(80,210-i/max_y*200,85,210-i/max_y*200,CL_BLACK);
			LCD_ShowNum(i,0,70,210-i/max_y*200-6,&tFont16);
		}
		max_y=max_y*pow(10,n);
		power = n;
		if(n!=0)
		{
			LCD_DispStr(82,0, "x",&tFont16);
			LCD_ShowNum(10,0,90,0,&tFont16);
			if(n<0)
			{
				LCD_DispStr(106,0, "-",&tFont12);
				power=-power;
				LCD_ShowNum(power,0,112,0,&tFont12);
			}
			else
				LCD_ShowNum(power,0,106,0,&tFont12);
		}
		
		symbol = 0;
		
		
		
		
		
		
		for(i=0;i<4096;i++)
		{
			S2[i]=0;
			S4[i]=0;
			y[i]=0;
			LCD_DrawLine((int)(i*720.0/4096.0)+79,210-(int)(x[i]*200.0/max_y),(int)((i+1)*720.0/4096.0)+79,210-(int)(x[i+1]*200.0/max_y),CL_BLUE);
		}
		i=(int)(20.0*4096/fs+1.5);
		/* Using Fast Fourier Transform to Calculate Order Spectrum */
		dfft(4096,12,S2,S4,x,y);
		k=i;
		for(i=0;i<k;i++)
		{
			S2[i]=sqrt(S2[i]*S2[i]+S4[i]*S4[i]);   //yft
			S2[i]=S2[i]*2.0/4096;
		}
		
		for(i=0;i<k;i++)
		{
			S4[i]=i;
			S4[i]=S4[i]*fs/4096.0;    //yf
		}
			
		for(i=2;i<k-1;i++)
		{
			if(S2[i]>max_f)
			{
				max_f=S2[i];

			}
		}

		if(max_f > 10)
		{
			symbol=1;
		}
		else if(max_f<1)
		{
			symbol=2;
		}
		n=0;
		if(symbol == 1)
		{
			while(max_f>10)
			{
				max_f/=10;
				n++;
			}
		}
		if(symbol==2)
		{
			while(max_f<1)
			{
				max_f*=10;
				n--;
			}
		}
		max_y=(int)(max_f);
		if (max_y<9)
			max_y+=1;
		for(i=1;i<=max_y;i++)
		{
			LCD_DrawLine(80,460-i/max_y*200,85,460-i/max_y*200,CL_BLACK);
			LCD_ShowNum(i,0,70,460-i/max_y*200-6,&tFont16);
		}
		max_y=max_y*pow(10,n);
		power = n;
		if(n!=0)
		{
			LCD_DispStr(82,460-200-28, "x",&tFont16);
			LCD_ShowNum(10,0,90,460-200-28,&tFont16);
		}
		if(n<0)
		{
			LCD_DispStr(106,460-200-30, "-",&tFont12);
			power=-power;
			LCD_ShowNum(power,0,112,460-200-30,&tFont12);
		}
		else
			LCD_ShowNum(power,0,106,460-200-30,&tFont12);

		for(i=1;i<k-1;i++)
		{
			LCD_DrawLine((int)(S4[i]*36)+79,460-(int)(S2[i]*200/max_y),(int)(S4[i+1]*36)+79,460-(int)(S2[i+1]*200/max_y),CL_BLUE);
		}

		HAL_Delay(500);
		LCD_ClrScr(CL_WHITE);
		time = bsp_GetRunTime() - time;
		printf("time = %dms\r\n",time);
	}
}