forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

Revision:
218:b4067cac75c0
Parent:
216:189b0ea1dc38
Child:
226:4a4d5bd5fcd7
--- a/vibro.c	Mon Sep 04 12:55:13 2017 +0000
+++ b/vibro.c	Mon Sep 25 10:35:56 2017 +0000
@@ -128,44 +128,49 @@
 { 
 
     int temp=0;
+    static unsigned int FConunt=0;
     int LowDZ,HiDZ;
+   /*	if(FConunt<4)
+   	{*/
+   		//FConunt++;
    	Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
-   // Gyro.CaunPlus = Gyro.CaunPlusReper;
+   	Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
 	CaunAddPlus = 0;
-	Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
-	//Gyro.CaunMin=Gyro.CaunMinReper;
 	CaunAddMin = 0;
 	Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; 
-	
-	
-	if(countFras<512)
-	{
-	  countFras++;
-	  Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
-	}
-	else
-	{   
-		Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
-		Gyro.F_ras=Gyro.F_rasAdd>>9;
-		Gyro.F_rasAdd=0;
-		countFras=0;
-	}
-	if(Gyro.RgConA&0x20)
-	{
+//	}
+	/*else
+		{*/
+		//	FConunt=0;
+		//Gyro.MaxAmp=Gyro.MaxAmp>>2;
+		if(countFras<512)
+			{
+			  	countFras++;
+	 			Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
+			}
+		else
+			{   
+				Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
+				Gyro.F_ras=Gyro.F_rasAdd>>9;
+				Gyro.F_rasAdd=0;
+				countFras=0;
+			}
+		if(Gyro.RgConA&0x20)
+			{
   	//расчет максимальной амплитуды из востановленного синуса р-р.
-  	temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed));
-  	temp=temp>>6;
-    LowDZ	=	((Gyro.AmpSpeed<<3)*(-1));
-    HiDZ	=	(Gyro.AmpSpeed<<3);
-    Gyro.Amp   -= temp; // расчет амплитуды ВП с учетом разници
-  		if((Gyro.Amp>>16) > (Gyro.AmpPerMax))   {Gyro.Amp = (Gyro.AmpPerMax << 16);}   // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
- 		if((Gyro.Amp>>16) < (Gyro.AmpPerMin))   {Gyro.Amp = (Gyro.AmpPerMin << 16);}  // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
+  				temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed));
+  				temp=temp>>6;
+   			 	LowDZ	=	((Gyro.AmpSpeed<<3)*(-1));
+    			HiDZ	=	(Gyro.AmpSpeed<<3);
+    			Gyro.Amp   -= temp>>4; // расчет амплитуды ВП с учетом разници
+  				if((Gyro.Amp>>16) > (Gyro.AmpPerMax))   {Gyro.Amp = (Gyro.AmpPerMax << 16);}   // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
+ 				if((Gyro.Amp>>16) < (Gyro.AmpPerMin))   {Gyro.Amp = (Gyro.AmpPerMin << 16);}  // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
        // Gyro.AmpPer = (Gyro.Amp)>>16; //приведение амплитуды ВП к виду 0%-100%
-    }
-    
-        	LPC_MCPWM->MAT1 = T_vib_1;
-        	LPC_MCPWM->MAT2 = T_vib_2;
-    }
+    		}
+       	LPC_MCPWM->MAT1 = T_vib_1;
+        LPC_MCPWM->MAT2 = T_vib_2;
+  	//	}
+ }