Diff: vibro.c

Revision:

191:40028201ddad

Parent:

190:289514f730ee

Child:

192:d32c8cf7bcd9

--- a/vibro.c	Tue Aug 23 14:03:16 2016 +0000
+++ b/vibro.c	Mon Aug 29 11:58:52 2016 +0000
@@ -4,8 +4,8 @@
 
 volatile unsigned int Cheng_AMP_Flag=0;
 //int    reper=0;
-int    Rate2VibFlag;
-
+int    Rate2VibFlag,countA=0,tempDP;
+unsigned int OldMaxAmp=0;
 void VibroOut(void) 	// выставка ног вибро
 {
     if(CountV31>=16) 
@@ -33,14 +33,57 @@
     }
 }
 
+
+
+
 void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
 {
-    
+    Gyro.AmpSC=0;
     static int PeriodCount = 0;
     unsigned int Nmax=0;
-
+    Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
+    if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
+    OldMaxAmp=Gyro.MaxAmp;
+    
+    if(Gyro.AmpSC <20)countA++;
+    if(countA >5)
+     { 
+      countA=0;
+        
+          Nmax   =   (unsigned int)((100000/(Gyro.Frq>>16))-1); 
+          Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16));            //левая граница амплитуды
+          Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1);                                //правая граница амплитуды      
+          Cheng_AMP_Flag=1;       
+          if(Gyro.LogHZ == 1)	//Запись для Ориджина.		
+   		    {
+       		 sprintf((Time),"%d \r\n",Gyro.AmpPerDel);
+       		 WriteCon(Time);
+    		} 
+        tempDP=Gyro.AmpPerDel;
+        srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
+        Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
+     }
+  
+    switch( Gyro.StrayHZ_flag) {
+    case 0:                              //8046
+    LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового  значение частоты вибро
+    break;
+    
+    case 1:
+    LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
+    break;
+    }
+}/*
+void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
+{
+    unsigned int OldMaxAmp=0;
+    static int PeriodCount = 0;
+    unsigned int Nmax=0;
+    Gyro.AmpSC = OldMaxAmp - Gyro.MaxAmp;
+    OldMaxAmp=Gyro.MaxAmp;
+    
     //расчет амплитуды относительно центральной точки
-    if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
+    if(Gyro.MaxAmp >= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
        PeriodCount=0;//сбрасываем таймер
        
         if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
@@ -78,8 +121,7 @@
     LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
     break;
     }
-}
-
+}*/
 void VibroAMPRegul(void)  //подстройка амплитуды ВП
 {
    	Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
@@ -88,19 +130,21 @@
 	CaunAddMin = 0;
 			
   	Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;          //расчет максимальной амплитуды из востановленного синуса р-р.
-    Gyro.Amp   -= (Gyro.MaxAmp - 3000) * 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.RgConA&0x20)     {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
-    
+    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.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++;} 
-    if(Gyro.RgConA&0x40)     {Gyro.Frq += TempFaza*1000;}
+    for (CountFaza = 0; CountFaza <8; CountFaza++ )     {if (Buff_Restored_sin [(CountV31 -12  + CountFaza) & 0xff] > 0 ) TempFaza++;} //резонанс когда  CountV31 = 8 => Buff_Restored_sin = 0
+    if(Gyro.RgConA&0x40)     {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;}
     
     if     (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее  ограничение частоты
     else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
@@ -113,19 +157,36 @@
 {  
     switch(CountV31) {
      case 0:
-          
+        	Gyro.VibroAMPRegulF=1;
             Time_vibro=0;
-            Gyro.VibroNoiseF=1;
+            Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
      break;
   
 	 case 16:
+	        /*if(Gyro.LogHZ == 1)	//Запись для Ориджина.		
+   		   {
+       		 sprintf((Time),"%d \r\n",Gyro.MaxAmp);
+       		 WriteCon(Time);
+    		}  */
             Time_vibro=0;
-            Gyro.VibroFrqRegulF=1;
+            Gyro.VibroFrqRegulF=1; //
             Gyro.Rate2_Event=1;
      break;
             
 	 case 24:
-			Gyro.VibroAMPRegulF=1;
+			
 	 break;
     }
-}
\ No newline at end of file
+}
+void AllRegul (void)
+{   ///////////////////////////контуры регулировки/////////////////////////////
+   
+    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.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0;	 VibroFrqRegul();	}	// Регулеровка частоты виброподвеса    
+    if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0;	 VibroAMPRegul();   }	// Регулеровка Амплитуды виброподвеса
+    if (Gyro.VibroNoiseF    == 1) {Gyro.VibroNoiseF = 0;	 CalcAmpN();	    }	// регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
+    if (Gyro.VibroOutF      == 1) {Gyro.VibroOutF = 0;		 VibroOut();		}	// установка ног в регисторе тоже подумать , зачем отделный флаг? наверно 
+
+}