sinsource.c

基于TMS320LF2407的一个倒立控制程序

#define ATurn_Rad  0.00322f  
 
extern float Phase;
extern int count;  
extern float f;
float sinsource(float A,float hz_end,float hz_delta,float T)
{ 
	static int Standardsiga[501]= {     0,       125,       251,       376,       502,       627,       753,       878,      1003,      1128,
      1253,      1377,      1502,      1626,      1750,      1873,      1997,      2120,      2242,      2364,
      2486,      2608,      2729,      2850,      2970,      3090,      3209,      3328,      3446,      3564,
      3681,      3797,      3913,      4029,      4143,      4257,      4371,      4483,      4595,      4707,
      4817,      4927,      5036,      5144,      5251,      5358,      5463,      5568,      5672,      5775,
      5877,      5979,      6079,      6178,      6276,      6374,      6470,      6565,      6660,      6753,
      6845,      6936,      7026,      7115,      7203,      7289,      7375,      7459,      7542,      7624,
      7705,      7784,      7862,      7939,      8015,      8090,      8163,      8235,      8305,      8375,
      8443,      8509,      8575,      8639,      8701,      8763,      8822,      8881,      8938,      8994,
      9048,      9101,      9152,      9202,      9250,      9297,      9343,      9387,      9429,      9470,
      9510,      9548,      9585,      9620,      9653,      9685,      9716,      9745,      9772,      9798,
      9822,      9845,      9866,      9886,      9904,      9921,      9936,      9949,      9961,      9971,
      9980,      9987,      9992,      9996,      9999,      9999,      9999,      9996,      9992,      9987,
      9980,      9971,      9961,      9949,      9936,      9921,      9904,      9886,      9866,      9845,
      9822,      9798,      9772,      9745,      9716,      9685,      9653,      9620,      9585,      9548,
      9510,      9470,      9429,      9387,      9343,      9297,      9250,      9202,      9152,      9101,
      9048,      8994,      8938,      8881,      8822,      8763,      8701,      8639,      8575,      8509,
      8443,      8375,      8305,      8235,      8163,      8090,      8015,      7939,      7862,      7784,
      7705,      7624,      7542,      7459,      7375,      7289,      7203,      7115,      7026,      6936,
      6845,      6753,      6660,      6565,      6470,      6374,      6276,      6178,      6079,      5979,
      5877,      5775,      5672,      5568,      5463,      5358,      5251,      5144,      5036,      4927,
      4817,      4707,      4595,      4483,      4371,      4257,      4143,      4029,      3913,      3797,
      3681,      3564,      3446,      3328,      3209,      3090,      2970,      2850,      2729,      2608,
      2486,      2364,      2242,      2120,      1997,      1873,      1750,      1626,      1502,      1377,
      1253,      1128,      1003,       878,       753,       627,       502,       376,       251,       125,
         0,      -125,      -251,      -376,      -502,      -627,      -753,      -878,     -1003,     -1128,
     -1253,     -1377,     -1502,     -1626,     -1750,     -1873,     -1997,     -2120,     -2242,     -2364,
     -2486,     -2608,     -2729,     -2850,     -2970,     -3090,     -3209,     -3328,     -3446,     -3564,
     -3681,     -3797,     -3913,     -4029,     -4143,     -4257,     -4371,     -4483,     -4595,     -4707,
     -4817,     -4927,     -5036,     -5144,     -5251,     -5358,     -5463,     -5568,     -5672,     -5775,
     -5877,     -5979,     -6079,     -6178,     -6276,     -6374,     -6470,     -6565,     -6660,     -6753,
     -6845,     -6936,     -7026,     -7115,     -7203,     -7289,     -7375,     -7459,     -7542,     -7624,
     -7705,     -7784,     -7862,     -7939,     -8015,     -8090,     -8163,     -8235,     -8305,     -8375,
     -8443,     -8509,     -8575,     -8639,     -8701,     -8763,     -8822,     -8881,     -8938,     -8994,
     -9048,     -9101,     -9152,     -9202,     -9250,     -9297,     -9343,     -9387,     -9429,     -9470,
     -9510,     -9548,     -9585,     -9620,     -9653,     -9685,     -9716,     -9745,     -9772,     -9798,
     -9822,     -9845,     -9866,     -9886,     -9904,     -9921,     -9936,     -9949,     -9961,     -9971,
     -9980,     -9987,     -9992,     -9996,     -9999,     -9999,     -9999,     -9996,     -9992,     -9987,
     -9980,     -9971,     -9961,     -9949,     -9936,     -9921,     -9904,     -9886,     -9866,     -9845,
     -9822,     -9798,     -9772,     -9745,     -9716,     -9685,     -9653,     -9620,     -9585,     -9548,
     -9510,     -9470,     -9429,     -9387,     -9343,     -9297,     -9250,     -9202,     -9152,     -9101,
     -9048,     -8994,     -8938,     -8881,     -8822,     -8763,     -8701,     -8639,     -8575,     -8509,
     -8443,     -8375,     -8305,     -8235,     -8163,     -8090,     -8015,     -7939,     -7862,     -7784,
     -7705,     -7624,     -7542,     -7459,     -7375,     -7289,     -7203,     -7115,     -7026,     -6936,
     -6845,     -6753,     -6660,     -6565,     -6470,     -6374,     -6276,     -6178,     -6079,     -5979,
     -5877,     -5775,     -5672,     -5568,     -5463,     -5358,     -5251,     -5144,     -5036,     -4927,
     -4817,     -4707,     -4595,     -4483,     -4371,     -4257,     -4143,     -4029,     -3913,     -3797,
     -3681,     -3564,     -3446,     -3328,     -3209,     -3090,     -2970,     -2850,     -2729,     -2608,
     -2486,     -2364,     -2242,     -2120,     -1997,     -1873,     -1750,     -1626,     -1502,     -1377,
     -1253,     -1128,     -1003,      -878,      -753,      -627,      -502,      -376,      -251,      -125,
         0}; 
   int iPhase;
   int P; 
   float w;    
   float SV,s1,s2,s3;
   w = 2*f;
   if(f<0.5)
     {    
      P = (int)(200/f);
      if(count>=P)
		{
		   w = 0; 
		   Phase = 0;
		   SV = 0;
		 }
	 if(count >=(P+200))
		{
		   count =count -(P+200);    	   
		   f = f+hz_delta;
		   w = 2*f;
		} 
       }
   else 
	   {	
	   if(count>=300)
		 {
		   w = 0; 
		  Phase = 0;
		  SV = 0;
		  }
	   if(count >=450)
		  {
		  	count =count -450;    	   
		   	f = f+hz_delta;
		   	w = 2*f;
		  } 
	  	}
		  	
   if(f >hz_end)
	 {
	   w = 0;
	 }   
		        
   if(Phase >=500) 
	 {
	    Phase =  Phase - 500;
	 }
   iPhase = (int)(Phase+0.5);   
   s1 = Standardsiga[iPhase];  
   s2 = A*s1;  
   s3=s2/10000;
   SV=s3/ATurn_Rad;
    
   Phase =Phase + w*T*250;     /*wt*/
   count = count + 1; 
   return SV;	  
}