Diff: QEI.c

Revision:

211:ac8251b067d2

Parent:

210:b02fa166315d

Child:

214:4c70e452c491

--- a/QEI.c	Mon Apr 24 05:43:54 2017 +0000
+++ b/QEI.c	Mon Jul 03 05:50:08 2017 +0000
@@ -5,11 +5,13 @@
 int FFF=0;//для заплатки
 
 unsigned int tempmod=64,tempmod2=1;
-unsigned int tempReper;
+unsigned int tempReper,tempReper2;
 int Pulse_8Point;
 int Pulse_16Point;
 int Pulse_32Point;
 int Pulse_64Point;
+int Pulse_96Point;
+int Pulse_128Point;
 int Pulse_16PointD;
 unsigned int CaunAddPlus	=0;//счетчик ипульсов энкодера"+" за такт ВП
 unsigned int CaunAddMin	    =0;//счетчик ипульсов энкодера"-" за такт ВП
@@ -18,7 +20,9 @@
 int temp32=0;
 int Buff_1Point			[512];			
 int Buff_32Point		[32];		
-int Buff_64Point		[32];	
+int Buff_64Point		[32];
+int Buff_96Point		[32];
+int Buff_128Point		[32];	
 int Buff_16Point		[32];
 int Buff_16PointD		[32];
 int Buff_8Point			[32];
@@ -39,8 +43,13 @@
     temp32=0;*/
     Last_QEI =  Cur_QEI;               // запись текущего значения энкодера в регистр предыдущего значения.
     
+   
+
     if (Dif_QEI < -0xfff)  Dif_QEI += 0x10000;   // обработка прохода значения через ноль
     if (Dif_QEI >  0xfff)  Dif_QEI -= 0x10000;    // обработка прохода значения через ноль
+    
+   /*  sprintf((Time)," %d %d %d \r\n",Gyro.CaunPlusReperAdd,Gyro.CaunMinReperAdd, Dif_QEI);
+     WriteCon(Time);*/
     Buff_1Point[CountV255] = Dif_QEI ; 	// накопление в буфер еденичных значений приращения по каждому такту.
     
  
@@ -60,9 +69,16 @@
         Buff_32Point[CountV31] = (Pulse_32Point );
                 
         Pulse_64Point += Buff_1Point[CountV255];
-        Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff];  								// заполнение буфера накопленых приращений за два 32 тактов
-       // Gyro.Cnt_Dif  =  (Pulse_64Point);
+        Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff];  								// заполнение буфера накопленых приращений за  64 тактов
         Buff_64Point[CountV31] = (Pulse_64Point );
+        
+        Pulse_96Point += Buff_1Point[CountV255];
+        Pulse_96Point -= Buff_1Point[(CountV255-96) & 0xff];  								// заполнение буфера накопленых приращений за 96 тактов
+        Buff_96Point[CountV31] = (Pulse_96Point );
+        
+        Pulse_128Point += Buff_1Point[CountV255];
+        Pulse_128Point -= Buff_1Point[(CountV255-128) & 0xff];  								// заполнение буфера накопленых приращений за 128 тактов
+        Buff_128Point[CountV31] = (Pulse_128Point );
 
 
         Pulse_16PointD += Buff_1Point[CountV255];
@@ -71,23 +87,33 @@
         Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff];									// 
         Buff_16PointD[CountV31] = Pulse_16PointD ;
      
-        Buff_Restored_sin  [CountV31] = (Buff_16PointD[CountV31]) - Buff_32Point[CountV31];
+        Buff_Restored_sin  [CountV31] = (Buff_16Point[CountV31])*2 - Buff_32Point[CountV31];
     
         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_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[CountV31]) > 0)       	Gyro.CaunPlusReperAdd  +=  Buff_1Point[(CountV255-7)&0xff];  // счетчик положительных импульсов
-    	else 								  	Gyro.CaunMinReperAdd   -=  Buff_1Point[(CountV255-7)&0xff];  // счетчик отрицательных импульсов // расчет амплитуды*/
+    	*/
+    
+     //   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: 
@@ -98,58 +124,51 @@
         break;
         /*   case 10:  
     	   
-    	    Gyro.CaunMinReper  = Gyro.CaunMinReperAdd;
-            Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
-            Gyro.CaunMinReperAdd  = 0; 
-            Gyro.CaunPlusReperAdd = 0;
-         sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper);
-   			 WriteCon(Time); 	*/
+
+            sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper);
+   			WriteCon(Time); 	*/
    			 
         break;
         }
     	if(Gyro.FrqPhase==CountV31)
     	{
-    		Gyro.CaunMinReper  = Gyro.CaunMinReperAdd;
+    	   //tempReper2=0;
+    	   /* Gyro.CaunMinReper  = Gyro.CaunMinReperAdd;
             Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
             Gyro.CaunMinReperAdd  = 0; 
-            Gyro.CaunPlusReperAdd = 0;
+            Gyro.CaunPlusReperAdd = 0;*/
         }
-    	if(Buff_Restored_sin [CountV31] > 0)	
-    	{
-    		if(Gyro.Reper_Event)tempReper=0;
-    		else         		tempReper=1;
-    	}
-    	else 				
-    	{
-    		if(tempReper)
-    		{ 
-    		
-    		
-
-     		  tempReper=0;
-    		  Gyro.Reper_Event=1;
-       
-
+        
+        
+        
+      if(Dif_QEI > 0)  
+      {
+      	if(tempReper)
+      	{
+      		Gyro.CaunMinReper  = Gyro.CaunMinReperAdd;
+	        Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
+	        Gyro.CaunMinReperAdd  = 0; 
+	        Gyro.CaunPlusReperAdd = 0;
+	        Gyro.Reper_Event=1;
+	      //  Gyro.CuruAngle  += Gyro.CaunPlusReper-Gyro.CaunMinReper;
+	        tempReper=0;
+	    }
+      	Gyro.CaunPlusReperAdd	+=	Dif_QEI;
+      }
+      else 
+      { 
+        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.CuruAngle  +=  Buff_32Point [CountV31];
-//LoopOff
 }
-/*
-void CurAngle(void)
-{
-	     for (int i = 0; i < 32; i++ ) 
-    	 {
-    	   Gyro.CuruAngle += Buff_32Point[i];
-    	 }
-}
-void CurAngleLOG(void)
-{
- sprintf((Time)," %d %d \r\n", Gyro.tempdelta,Gyro.CuruAngle);
-     WriteCon(Time); 	
-     Gyro.CuruAngle=0;
-     Gyro.tempdelta=0;
- }*/
+
      
\ No newline at end of file