vibro.c

00001 #include "Global.h"
00002 
00003 
00004 struct MAIN Main;
00005 
00006 
00007 volatile int V1 = 0 ;
00008 volatile int Temp = 0 ;
00009 volatile unsigned int Flag=0;
00010 
00011 unsigned int FrecTemp=0;
00012 /////////////////////////////////////////////////////////////////////////////
00013 /////////////////////////инициализация вибропривода//////////////////////////
00014 /////////////////////////////////////////////////////////////////////////////
00015 void init_Vibro ()
00016 {
00017     Main.FrqRate=40;
00018     Main.FrqMin=380;
00019     Main.FrqHZ=449;
00020     Main.Frq = Main.FrqHZ<<16;
00021 
00022     Main.AmpPer=20;
00023     Main.AmpPerDel=10;
00024     Main.Amp  = 10*65535;
00025     Main.AmpL = 20*650;
00026     /*
00027                   Vib.AmpH=50*650;
00028             //  Vib.AmpT=600;
00029             //  Output.Str.T_Vibro=40;
00030             //LPC_GPIO0->FIOSET   |= (1<<5);
00031     */
00032 }
00033 
00034 
00035 void VibroOut(void)
00036 {
00037 
00038 
00039     if(CountV31>=16) {
00040         if((Time_vibro>Main.AmpN1) && (Time_vibro<Main.AmpN2))  {
00041             ClrV2 /*LoopOn*/
00042         } else {
00043             SetV2
00044         }
00045     } else {
00046         if((Time_vibro>Main.AmpN1)&&(Time_vibro<Main.AmpN2))    {
00047             ClrV1 /*LoopOff*/
00048         } else {
00049             SetV1
00050         }
00051     }
00052 
00053 
00054 
00055 }
00056 
00057 void CalcAmpN(void)
00058 {
00059     static int PeriodCount = 0,Tnoise;
00060 
00061 
00062 
00063     unsigned int Nmax=0;
00064 
00065 
00066     if(PeriodCount>= Tnoise) { //если количество заходов в прерывание больше либо равно частоте ошумления.
00067         if (Flag==0) { //сейчас малая амплитуда?
00068 
00069             if(Main.AmpPer>90) {
00070                 Main.AmpPer=90;
00071             }
00072             Nmax =(unsigned int)((100000/(Main.Frq>>16))-1);
00073             Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer))/400);
00074             Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1);
00075             Flag=1;
00076 
00077         }
00078 
00079         else {
00080             if((Main.AmpPer+Main.AmpPerDel)>90) {
00081                 Main.AmpPer=90-Main.AmpPerDel;
00082             }
00083 
00084             Nmax =(unsigned int)((100000/(Main.Frq>>16))-1);
00085             Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer+Main.AmpPerDel))/400);
00086             Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1);
00087             Flag=0;
00088         }
00089         Main.AmpMin =10;
00090         Main.AmpTD =30;
00091         //Tnoise=55;
00092         srand(Global_Time);
00093         Main.AmpT = (rand() % Main.AmpTD+Main.AmpMin);// ОШУМЛЕНИЕ amp
00094         Tnoise=Main.AmpT;
00095         //изменение флага предидущей амплитуды
00096         PeriodCount=0;
00097     } else {
00098         PeriodCount++;
00099     }
00100 
00101 
00102 
00103 }
00104 
00105 //////////////////////////////////////////////////////////////////////////////
00106 /////////////////////////функция работы вибропривода//////////////////////////
00107 //////////////////////////////////////////////////////////////////////////////
00108 void cheng(void)
00109 {
00110     static int TempFaza, CountFaza;
00111 
00112 
00113     //Noise();
00114 
00115     //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
00116     /*
00117     //  CountV++;
00118     //  CountV &= 0x1f;
00119     */
00120     if (Buff_Restored_sin [CountV255] > MaxAmp) {
00121         MaxAmp=Buff_Restored_sin [CountV255];
00122     }
00123 
00124 
00125 
00126 
00127 
00128     switch(CountV31) {
00129         case 0:
00130             CalcAmpN();
00131             //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16;
00132             LPC_TIM1->MR0 =(unsigned int)(100000000/(Main.Frq>>11));
00133             Output.Str.T_Vibro=(unsigned int)((7680000*16/Main.Frq)*4096);
00134             VibroPush();
00135             //  LoopOn
00136             //  LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq);
00137             Time_vibro=0;
00138             break;
00139         case 10:
00140 
00141 //          if (MaxAmp>(65535+400))
00142 //          {
00143 //              Vib.Amp-=1000;//
00144 //          }
00145 //          else
00146 //          {
00147 //              Vib.Amp+=1000;
00148 //          }
00149 
00150 
00151 
00152 
00153             Main.Amp -= (MaxAmp - 65536 - 300)*4;  // расчет амплитудв с учетом разници
00154             ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00155             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00156             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00157             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00158             if(Main.Amp>2500000) {
00159                 Main.Amp=1200000;   // временное ограничение роста амплитуды в случае неподоженного гироскопа//////////
00160             }
00161             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00162             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00163             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00164             ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00165 
00166 
00167             Main.AmpPer = Main.Amp>>16;
00168             MaxAmp=0;
00169             FrecTemp=Main.AmpPer;//для вывода в терминал
00170             break;
00171         case 16:
00172             VibroPull();
00173             //SetV2
00174             // LoopOff
00175             Time_vibro=0;
00176 
00177 
00178 
00179 
00180 
00181 
00182 // расчет Фазы с учетор разници
00183             TempFaza = -4;
00184             for (CountFaza = 0; CountFaza < 8; CountFaza++ ) {
00185                 if (Buff_Restored_sin [(CountV255- 12  + CountFaza) & 0xff] > 65535) TempFaza++;
00186             } // расчет Фазы с учетор разници
00187             Main.Frq -=TempFaza*1000;
00188 
00189 
00190 
00191 
00192 //          if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535)
00193 //          {
00194 //          Vib.Frq=Vib.Frq-10000;
00195 //          }
00196 
00197 //          else
00198 //          {
00199 //              Vib.Frq=Vib.Frq+1000;
00200 //          }
00201             break;
00202     }
00203 
00204 
00205 
00206 
00207     /*
00208     if((CountV & 0x0f) == V1)
00209         {
00210             ClrV1
00211             ClrV2
00212         }*/
00213 }