vibro.c

00001 #include "Global.h"
00002 GyroT Gyro;
00003 GyroParam GyroP;
00004 
00005 TermCorS TermoCorrStatic;
00006 TermCorD TermoCorrDynamic;
00007 
00008 
00009 volatile unsigned int Cheng_AMP_Flag=0;
00010 //int    reper=0;
00011 int    Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo;
00012 unsigned int OldMaxAmp=0,countFras=0;
00013 int z=25;
00014 int i=16,tempi=0,klk=0;
00015 
00016 
00017 __irq void EINT3_IRQHandler()
00018 {  
00019     if( Gyro.EXT_Latch){}
00020     else   Gyro.EXT_Latch=1; 
00021     LPC_GPIOINT->IO0IntClr |= (1<<1);
00022     //NVIC_DisableIRQ(EINT3_IRQn);
00023       
00024 }
00025 
00026 
00027 void VibroOut(void)     // выставка ног вибро
00028 {
00029     if(CountV31>=16) 
00030       {//первая нога вибро
00031          // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ  граница вЫкл вибро 1
00032         if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2))  
00033         {
00034             SetV1//установить в регистре PinReg бит "вибро 1" в "0"
00035         } 
00036         else 
00037         {
00038              ClrV1  //установить в регистре PinReg бит "вибро 1" в "1"
00039         }
00040 
00041       } 
00042     else {//вторая нога вибро
00043         if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2))    
00044         {
00045              SetV2 //установить в регистре PinReg бит "вибро 2" в "0"
00046         } 
00047         else
00048         {
00049             ClrV2//установить в регистре PinReg бит "вибро 2" в "1"
00050         }
00051     }
00052 }
00053 /*
00054 
00055 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
00056 {   
00057     static int PeriodCount = 0,Period=0;
00058     unsigned int Nmax=0, lowper=1;
00059     Gyro.FrqHZ=Gyro.Frq>>16;
00060     if(PeriodCount>= Gyro.AmpT) 
00061     { //если количество заходов в прерывание больше либо равно частоте ошумления.
00062       PeriodCount=0;//сбрасываем таймер
00063          sprintf((Time),"%d   %d    %d\r\n", Gyro.AmpN1, Gyro.AmpN2-Gyro.AmpN1,  Gyro.AmpPer);
00064          WriteCon(Time);
00065     //Gyro.AmpPerDel=(Gyro.AmpPer*100)/Gyro.AmpPerDel;   
00066     if(Cheng_AMP_Flag==0) 
00067       { 
00068       if((Gyro.AmpPer+Gyro.AmpPerDel)>90)             Gyro.AmpPer=90-Gyro.AmpPerDel;   //проверка верхней граници амплитуды
00069        Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);//256                    //
00070        Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ);     //левая граница амплитуды  63
00071        Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1);                     //правая граница амплитуды 65
00072        Gyro.L_vibro=Gyro.AmpPer*208;
00073        sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag);
00074        WriteCon(Time);  
00075        Cheng_AMP_Flag=1;
00076        }
00077     else  
00078        {
00079         if((Gyro.AmpPer+Gyro.AmpPerDel)>90)          Gyro.AmpPer=90-Gyro.AmpPerDel;   //проверка верхней граници амплитуды
00080         if((Gyro.RgConA&0x20))
00081           {            
00082             Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);//256
00083             Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/Gyro.FrqHZ); //левая граница амплитуды 61
00084             Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1);                                //правая граница амплитуды 67
00085             Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*208;
00086             sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag);
00087             WriteCon(Time); 
00088           }
00089         Cheng_AMP_Flag=0;
00090         }
00091     if(Gyro.AmpN2<(Gyro.AmpN1+2))Gyro.AmpN2=Gyro.AmpN1+2;
00092     
00093       srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
00094      Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
00095     } else {
00096         PeriodCount++;//таймер амплитуды
00097     }
00098 }*/
00099 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
00100 {   
00101     static int PeriodCount = 0,Period=0;
00102     unsigned int Nmax=0, lowper=1;
00103     Gyro.FrqHZ=Gyro.Frq>>16;
00104     if(PeriodCount>= Gyro.AmpT) 
00105     { 
00106       PeriodCount=0;//сбрасываем таймер
00107       if(Cheng_AMP_Flag==0) 
00108         { 
00109                 //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000)             Gyro.AmpPer=9000-Gyro.AmpPerDel*100;   //проверка верхней граници амплитуды
00110                 T_vib_1 = Gyro.AmpPer * T_vibP;
00111                 T_vib_2 = T_vibP * (10000-Gyro.AmpPer);
00112                 Gyro.L_vibro=Gyro.AmpPer*3;
00113                 Cheng_AMP_Flag=1;
00114         }
00115       else  
00116        {
00117                 //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000)          Gyro.AmpPer=9000-Gyro.AmpPerDel*100;   //проверка верхней граници амплитуды
00118                 if((Gyro.RgConA&0x20))
00119                     {            
00120                         T_vib_1 = T_vibP * (Gyro.AmpPer +(Gyro.AmpPerDel*100));
00121                         T_vib_2 = T_vibP * (10000 -(Gyro.AmpPer+(Gyro.AmpPerDel*100)));
00122                         Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*3;
00123                     }
00124                  Cheng_AMP_Flag=0;
00125         }
00126 
00127             srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
00128             Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
00129      }
00130      else {PeriodCount++;}
00131 
00132 }
00133 
00134 void VibroAMPRegul(void)  //подстройка амплитуды ВП
00135 { 
00136 
00137     int temp=0;
00138     static unsigned int FConunt=0;
00139     int LowDZ,HiDZ;
00140    /*   if(FConunt<4)
00141     {*/
00142         //FConunt++;
00143     Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
00144     Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
00145     CaunAddPlus = 0;
00146     CaunAddMin = 0;
00147     Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; 
00148 //  }
00149     /*else
00150         {*/
00151         //  FConunt=0;
00152         //Gyro.MaxAmp=Gyro.MaxAmp>>2;
00153         if(countFras<512)
00154             {
00155                 countFras++;
00156                 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
00157             }
00158         else
00159             {   
00160                 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40;
00161                 Gyro.F_ras=Gyro.F_rasAdd>>9;
00162                 Gyro.F_rasAdd=0;
00163                 countFras=0;
00164             }
00165         if(Gyro.RgConA&0x20)
00166             {
00167     //расчет максимальной амплитуды из востановленного синуса р-р.
00168                 temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed));
00169                 temp=temp>>6;
00170                 LowDZ   =   ((Gyro.AmpSpeed<<3)*(-1));
00171                 HiDZ    =   (Gyro.AmpSpeed<<3);
00172                 Gyro.Amp   -= temp>>4; // расчет амплитуды ВП с учетом разници(Gyro.AmpPer<<17)/100;
00173                 if((Gyro.AmpPer) > (Gyro.AmpPerMax))   {Gyro.Amp = ((Gyro.AmpPerMax<<17)/100);}   // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
00174                 if((Gyro.AmpPer) < (Gyro.AmpPerMin))   {Gyro.Amp = ((Gyro.AmpPerMin<<17)/100);}  // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
00175        // Gyro.AmpPer = (Gyro.Amp)>>16; //приведение амплитуды ВП к виду 0%-100%
00176             }
00177         LPC_MCPWM->MAT1 = T_vib_1;
00178         LPC_MCPWM->MAT2 = T_vib_2;
00179     //  }
00180  }
00181 
00182 
00183 
00184 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
00185 { 
00186     static int TempFaza, CountFaza;
00187     TempFaza = -4;
00188     Gyro.FrqPhaseEror=0;
00189     for (CountFaza = 0; CountFaza <8; CountFaza++ )     {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase  + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда  CountV31 = 8 => Buff_Restored_sin = 0
00190     for (CountFaza = 0; CountFaza <8; CountFaza++ )     {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase  + CountFaza) & 0x1f];}
00191     if(Gyro.RgConA&0x40)
00192     {                                                                                       //12
00193       Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;
00194     }
00195   //  Gyro.FrqPhaseEror = TempFaza<<10;
00196     if     (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее  ограничение частоты
00197     else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты*/
00198     LPC_TIM1->MR0 =(unsigned int)(103200000/(Gyro.Frq>>11));//запись в таймер нового  значение частоты вибро
00199 }
00200 
00201 //////////////////////////////////////////////////////////////////////////////
00202 /////////////////////////основного 32 тактного цикла//////////////////////////
00203 //////////////////////////////////////////////////////////////////////////////
00204 void cheng(void)
00205 { 
00206   switch(CountV31) {
00207      case 0:
00208             ReVib();///обновление значений вибро
00209             Gyro.VibroAMPRegulF=1;
00210             Time_vibro=0;
00211             Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.  
00212      break;
00213      case 8:
00214         LPC_MCPWM->CON_CLR |= (1<<8);
00215         LPC_MCPWM->CON_CLR |= (1<<16);
00216        
00217          LPC_MCPWM->TC1 =1;
00218          LPC_MCPWM->TC2 =1;
00219       
00220          LPC_MCPWM->CON_SET |= (1<<8);
00221          LPC_MCPWM->CON_SET |= (1<<16);  //вкл
00222         break;
00223         
00224      case 16:
00225          Time_vibro=0;
00226          Gyro.VibroFrqRegulF=1; //
00227      break;
00228      
00229      case 31:
00230 
00231      break;
00232      }
00233 }
00234 void AllRegul (void)
00235 {   ///////////////////////////контуры регулировки/////////////////////////////
00236    
00237     if (Spi.ADC_NewData     == 1) {ADS_Acum();                                  }   // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
00238     if (Gyro.ADF_NewData    == 1) {Gyro.ADF_NewData = 0;                        }   // был приход новых данных После быстрого фильтра AD    
00239     if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0;  VibroFrqRegul();   }   // Регулеровка частоты виброподвеса    
00240     if (Gyro.VibroAMPRegulF == 1) 
00241     {
00242         Gyro.VibroAMPRegulF = 0;    
00243         VibroAMPRegul(); 
00244         PLCRegul();
00245         HFORegul();
00246     }   // Регулеровка Амплитуды виброподвеса
00247     if (Gyro.VibroNoiseF    == 1) {Gyro.VibroNoiseF = 0;  OLDCalcAmpN();}
00248 }