Diff: QEI.c

Revision:

231:079835d508ef

Parent:

214:4c70e452c491

Child:

232:130a2b5003e6

--- a/QEI.c	Tue Jan 09 13:25:45 2018 +0000
+++ b/QEI.c	Thu Jan 25 13:59:43 2018 +0000
@@ -1,4 +1,5 @@
 #include "Global.h"
+//#include "math.h"
 
 int Dif_QEI;
 
@@ -15,7 +16,7 @@
 int Pulse_16PointD;
 unsigned int CaunAddPlus	=0;//счетчик ипульсов энкодера"+" за такт ВП
 unsigned int CaunAddMin	    =0;//счетчик ипульсов энкодера"-" за такт ВП
-
+int TempAngelCompens=0;
 int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера
 int temp32=0;
 int Buff_1Point			[512];			
@@ -28,8 +29,48 @@
 int Buff_8Point			[32];
 int Buff_Restored_sin 	[32];
 int Buff_Restored_sin2 	[32];
+
+	/* float i = 0.123;	 
+	   float j = 1.00000000000;
+	   static int SumTermoCompens=0,TermoCompens=0;
+	   int TermoCompensCalc;
+	   TermoCompensCalc = (TermoComp<<22)/(100 * Frq);
+	   SumTermoCompens 	 += TermoCompensCalc;
+	   Gyro.AngelCompens += SumTermoCompens>>22;
+       SumTermoCompens   -= SumTermoCompens&0xffc00000;*/
+       
+       
+int TermoCopensRecalc(unsigned int T4, unsigned int T5)
+{
+	   static int OldTempT4=0,OldTempT5=0,OldTempAVR;
+	   static int TempAVR;
+	   static unsigned int FlagRecalc=0;
+	   int DeltaT4, DeltaT5;
+	   
+	   DeltaT4=Spi.ADC1-OldTempT4;
+	   DeltaT4=abs(DeltaT4);
+	   if(DeltaT4>15) 
+	   {
+	   	sprintf((Time),"Recalc\r\n");
+        WriteCon(Time);
+	   	OldTempT4=Spi.ADC1;
+	   	FlagRecalc=1;
+	   }
+	      
+	   DeltaT5=Spi.ADC2-OldTempT5;
+	   DeltaT5=abs(DeltaT5);
+	   if(DeltaT5>15)
+	   {
+	   	OldTempT5=Spi.ADC2;
+	   }
+
+	   return Gyro.AngelCompens ;
+}
+
+
 void D_QEI(void)
 {
+ 
   
     Dif_QEI=0;
     Cur_QEI  =  LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
@@ -68,7 +109,7 @@
         Gyro.Cnt_Dif  =  (Pulse_32Point);
         Buff_32Point[CountV31] = (Pulse_32Point );
                 
-        Pulse_64Point += Buff_1Point[CountV255];
+       /* Pulse_64Point += Buff_1Point[CountV255];
         Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff];  								// заполнение буфера накопленых приращений за  64 тактов
         Buff_64Point[CountV31] = (Pulse_64Point );
         
@@ -85,7 +126,7 @@
         Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff];									// заполнение буфера накопленых приращений за 16 тактов Двойныз
         Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff];									//
         Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff];									// 
-        Buff_16PointD[CountV31] = Pulse_16PointD ;
+        Buff_16PointD[CountV31] = Pulse_16PointD ;*/
      
      
      
@@ -102,29 +143,7 @@
         if((Buff_Restored_sin [CountV31]) > 0)	CaunAddPlus += Buff_Restored_sin [CountV31];  // счетчик положительных импульсов
     	else 									CaunAddMin  -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды
     	
-    	
- //   	if((Buff_1Point[CountV31]) > 0)    	    Gyro.CaunPlusRateAdd  +=  Buff_1Point[CountV255];  // счетчик положительных импульсов
- //   	else 									Gyro.CaunMinRateAdd   -=  Buff_1Point[CountV255];  // счетчик отрицательных импульсов // расчет амплитуды
-    	/*
-    	if(Buff_Restored_sin [CountV31] > 0)    Gyro.CaunPlusReperAdd  +=  Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f];  // счетчик положительных импульсов
-    	else 								  	Gyro.CaunMinReperAdd   -=  Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f];  // счетчик отрицательных импульсов // расчет амплитуды
-    	*/
-    
-     //   if(Buff_Restored_sin [CountV31]>0)  	    Gyro.CaunPlusReperAdd  +=  Buff_1Point[(CountV255 - 8 /*Gyro.FrqPhase*/)&0xff];  // счетчик положительных импульсов
-     //   else 									  	Gyro.CaunMinReperAdd   -=  Buff_1Point[(CountV255 - 8 /* Gyro.FrqPhase*/)&0xff];  // счетчик отрицательных импульсов // расчет амплитуды
-    
-    	//if((Buff_1Point[CountV255]) > 0)       	Gyro.CaunPlusReperAdd  +=  Buff_1Point[(CountV255)&0xff];  // счетчик положительных импульсов
-    	//else 								  	Gyro.CaunMinReperAdd   -=  Buff_1Point[(CountV255)&0xff];  // счетчик отрицательных импульсов // расчет амплитуды*/
-    	
-    	
-  //  	if (LPC_QEI->STAT)	 Gyro.CaunMinReperAdd -= Dif_QEI;
-  //	else 				 Gyro.CaunPlusReperAdd += Dif_QEI;
-		
 
-    	
-    	
-    	
-    	
     	switch(CountV31){
         case 31: 
         Gyro.CaunMinRate = Gyro.CaunMinRateAdd;
@@ -132,24 +151,10 @@
         Gyro.CaunMinRateAdd  = 0; 
         Gyro.CaunPlusRateAdd = 0;
         break;
-        /*   case 10:  
-    	   
 
-            sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper);
-   			WriteCon(Time); 	*/
-   			 
         break;
         }
-    	if(Gyro.FrqPhase==CountV31)
-    	{
-    	   //tempReper2=0;
-    	   /* Gyro.CaunMinReper  = Gyro.CaunMinReperAdd;
-            Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
-            Gyro.CaunMinReperAdd  = 0; 
-            Gyro.CaunPlusReperAdd = 0;*/
-        }
-        
-        
+      
         
       if(Dif_QEI > 0)  
       {
@@ -169,16 +174,17 @@
       { 
         tempReper=1;
         Gyro.CaunMinReperAdd		-=	Dif_QEI;
-       	
       }
       
-    	Gyro.CuruAngle  	+=  Buff_32Point [CountV31];  
-    	Gyro.CuruAngle64  	+=  Buff_64Point [CountV31];
-		Gyro.CuruAngle96	+=  Buff_96Point [CountV31]; 
-		Gyro.CuruAngle128	+=  Buff_128Point [CountV31];  	
- 	
-	
+      
+      
+       
+	 //  Gyro.TermoModAdd=TermoCopensRecalc(Gyro.CurTermoCompens, Gyro.Frq>>11);
 
+        
+       Gyro.CuruAngle  	+=  Buff_32Point [CountV31];  
+  
 }
 
+
      
\ No newline at end of file