Diff: vibro.c

Revision:

196:f76dbc081e63

Parent:

194:8f3cb37a5541

Child:

197:7a05523bf588

--- a/vibro.c	Tue Sep 20 10:39:30 2016 +0000
+++ b/vibro.c	Fri Sep 23 05:34:50 2016 +0000
@@ -87,7 +87,7 @@
     if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
     OldMaxAmp=Gyro.MaxAmp;
     
-    if(Gyro.AmpSC <20)countA++;
+    if(Gyro.AmpSC <5)countA++;
     if(countA >3)
      { 
       countA=0;
@@ -135,9 +135,7 @@
      	if (Gyro.AmpPerDel<1)Znak=0;  
      	if (fnoize>6)Znak=2;
      	break;
-     
-     
-     
+
      	case 2:
      	Gyro.AmpPerDel++;
      	if (Gyro.AmpPerDel>7){Znak=3; fnoize++;}
@@ -147,11 +145,7 @@
 		if (Gyro.AmpPerDel<1)Znak=2;  
 		if (fnoize>12){Znak=4;/*fnoize=0;*/}
    		break;
-   	
-   	
-   	
-   	
-   		
+
    		case 4:
      	Gyro.AmpPerDel++;
 		if (Gyro.AmpPerDel>15){Znak=5; fnoize++;} 
@@ -161,12 +155,7 @@
 		if (Gyro.AmpPerDel<1)Znak=4;  
 		if (fnoize>18){Znak=6;/*fnoize=0;*/}
    		break;
-   
-   
-   
-   
-   
-   		
+
      	case 6:
      	Gyro.AmpPerDel++;
      	if (Gyro.AmpPerDel>6){Znak=7;fnoize++;}  
@@ -263,15 +252,11 @@
     } else {
         PeriodCount++;//таймер амплитуды
     }
-    switch( Gyro.StrayHZ_flag) {
-    case 0:
-    LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового  значение частоты вибро
-    break;
     
-    case 1:
-    LPC_TIM1->MR0 =(unsigned int)((100000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
-    break;
-    }
+LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового  значение частоты вибро
+  
+	
+    
 }
 void VibroAMPRegul(void)  //подстройка амплитуды ВП
 {
@@ -283,15 +268,16 @@
   	Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;         //расчет максимальной амплитуды из востановленного синуса р-р.
     Gyro.Amp   -= (Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed;  // расчет амплитуды ВП с учетом разници
   if((Gyro.Amp>>16) > Gyro.AmpPerMax)   {Gyro.Amp = (Gyro.AmpPerMax << 16);}   // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
-  if((Gyro.Amp>>16) > Gyro.AmpPerMin)   {Gyro.Amp = (Gyro.AmpPerMin << 16);}  // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
+  if((Gyro.Amp>>16) < Gyro.AmpPerMin)   {Gyro.Amp = (Gyro.AmpPerMin << 16);}  // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
   if(Gyro.RgConA&0x20)     {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
   //  Gyro.MaxAmp =0;
+
 }
 
 
 
 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
-{
+{ 
    	static int TempFaza, CountFaza;
    	TempFaza = -4;
     for (CountFaza = 0; CountFaza <8; CountFaza++ )     {if (Buff_Restored_sin [(CountV31 -12  + CountFaza) & 0xff] > 0 ) TempFaza++;} //резонанс когда  CountV31 = 8 => Buff_Restored_sin = 0
@@ -308,19 +294,34 @@
 {  
     switch(CountV31) {
      case 0:
-            //CuruAngle();//СЧЕТ УГЛА ПО 32 ТОЧКАМ.
-        	Gyro.VibroAMPRegulF=1;
+          	Gyro.VibroAMPRegulF=1;
             Time_vibro=0;
             Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
+
      break;
+    
 	 case 16:
 	        //Gyro.Reper_Event=1; 
             Time_vibro=0;
             Gyro.VibroFrqRegulF=1; //
      break;
-            
-	 case 31:
-	 //CuruAngleLOG();
+     
+     case 31:
+     for (int i = 0; i < 32; i++ ) 
+     { 
+      Gyro.CuruAngle  +=  Buff_32Point [i];
+      Gyro.tempdelta  += (Buff_Restored_sin[i]);
+      Gyro.tempdelta2 += (Buff_Restored_sin2[i]);
+     }
+     if(Gyro.LogHZ)
+     {
+	 sprintf((Time),"%d %d %d\r\n",	Gyro.CuruAngle,Gyro.tempdelta,Gyro.tempdelta2);  WriteCon(Time);
+	 Gyro.CuruAngle=0;
+	 Gyro.tempdelta=0;
+	 Gyro.tempdelta2=0;
+	
+     }
+	
      break;
     }
 }
@@ -329,38 +330,48 @@
    
     if (Spi.ADC_NewData     == 1) {ADS_Acum();								 	}   // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
 	if (Gyro.ADF_NewData    == 1) {Gyro.ADF_NewData = 0;                    	}   // был приход новых данных После быстрого фильтра AD	
-	if (Gyro.ADS_NewData    == 1) {Gyro.ADS_NewData = 0;  if(Gyro.ModJump == 3){ShowMod();}  else {PlcRegul();}}   // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
+	if (Gyro.ADS_NewData    == 1) 
+	{
+		Gyro.ADS_NewData = 0; 
+		switch(Gyro.LogPLC) {
+        case 0:     PlcRegul();    break;    
+	    case 1:     PlcRegul();    break;
+	    case 2:     ShowMod();     break;
+	    }
+	}   // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
     if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0;	 VibroFrqRegul();	}	// Регулеровка частоты виброподвеса    
     if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0;	 VibroAMPRegul();   }	// Регулеровка Амплитуды виброподвеса
     if (Gyro.VibroNoiseF    == 1) 
     { 
-   
-    if(Gyro.flag==2)
-    {
-    //Gyro.AmpPer=20;
-    Gyro.AmpPerDel=10;
+  /*Gyro.AmpPerDel=10;
     Gyro.AmpMin =10;// минимальное  значение AmpT;
     Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления)
+    Gyro.VibroNoiseF = 0;
+    OLDCalcAmpN();*/
+   
+   switch(Gyro.flag) {
+     case 1:
+   Gyro.VibroNoiseF = 0;
     OLDCalcAmpN();
-    Gyro.VibroNoiseF = 0;
-    }
-    else if(Gyro.flag==3)
-    {
-    	Gyro.AmpMin =3;
+    break;
+    
+    case 2:
+      	Gyro.AmpMin =3;
     	Gyro.AmpTD =10;
     	Gyro.VibroNoiseF = 0;	
-    	Calc2AmpN();	   
-    } 
-    else
-    {
+    	Calc2AmpN();	  
+    break;
+    
+    case 3:
     	Gyro.AmpMin =1;
     	Gyro.AmpTD =10;
     	Gyro.VibroNoiseF = 0;	
     	//CalcAmpN();
     	Gyro.AmpTarget =15000;	
-    	CalcAmpI();  
-    	//CalcAmpD();
-    } 
+    	//CalcAmpI();  
+    	CalcAmpD();  
+    break; 
+}
     }	// регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
     if (Gyro.VibroOutF      == 1) {Gyro.VibroOutF = 0;		 VibroOut();		}	// установка ног в регисторе тоже подумать , зачем отделный флаг? наверно