123
Fork of LG by
vibro.c
- Committer:
- igor_v
- Date:
- 2016-02-05
- Revision:
- 28:17c84ed091b3
- Parent:
- 21:bc8c1cec3da6
- Child:
- 33:341521841d3a
File content as of revision 28:17c84ed091b3:
#include "Global.h" GyroT Gyro; volatile int V1 = 0 ; volatile int Temp = 0 ; volatile unsigned int Flag=0; unsigned int FrecTemp=0; ///////////////////////////////////////////////////////////////////////////// /////////////////////////инициализация вибропривода////////////////////////// ///////////////////////////////////////////////////////////////////////////// unsigned int init_Vibro () { Gyro.FrqRate=40; Gyro.FrqMin=380; Gyro.FrqHZ=449; Gyro.Frq = Gyro.FrqHZ<<16; Gyro.AmpPer=20; Gyro.AmpPerDel=10; Gyro.Amp = 10*65535; Gyro.AmpL = 20*650; /* Vib.AmpH=50*650; // Vib.AmpT=600; // Output.Str.T_Vibro=40; //LPC_GPIO0->FIOSET |= (1<<5); */ return 0; } void VibroOut(void) { if(CountV31>=16) { if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { ClrV2 /*LoopOn*/ } else { SetV2 } } else { if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { ClrV1 /*LoopOff*/ } else { SetV1 } } } void CalcAmpN(void) { static int PeriodCount = 0,Tnoise; unsigned int Nmax=0; if(PeriodCount>= Tnoise) { //если количество заходов в прерывание больше либо равно частоте ошумления. if (Flag==0) { //сейчас малая амплитуда? if(Gyro.AmpPer>90) { Gyro.AmpPer=90; } Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/400); Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); Flag=1; } else { if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { Gyro.AmpPer=90-Gyro.AmpPerDel; } Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/400); Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); Flag=0; } Gyro.AmpMin =10; Gyro.AmpTD =30; //Tnoise=55; srand(Global_Time); Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp Tnoise=Gyro.AmpT; //изменение флага предидущей амплитуды PeriodCount=0; } else { PeriodCount++; } } ////////////////////////////////////////////////////////////////////////////// /////////////////////////функция работы вибропривода////////////////////////// ////////////////////////////////////////////////////////////////////////////// void cheng(void) { static int TempFaza, CountFaza; //Noise(); //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; /* // CountV++; // CountV &= 0x1f; */ if (Buff_Restored_sin [CountV255] > MaxAmp) { MaxAmp=Buff_Restored_sin [CountV255]; } switch(CountV31) { case 0: CalcAmpN(); //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11)); Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); VibroPush(); // LoopOn // LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq); Time_vibro=0; break; case 10: // if (MaxAmp>(65535+400)) // { // Vib.Amp-=1000;// // } // else // { // Vib.Amp+=1000; // } Gyro.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// if(Gyro.Amp>2500000) { Gyro.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Gyro.AmpPer = Gyro.Amp>>16; MaxAmp=0; FrecTemp=Gyro.AmpPer;//для вывода в терминал break; case 16: VibroPull(); //SetV2 // LoopOff Time_vibro=0; // расчет Фазы с учетор разници TempFaza = -4; for (CountFaza = 0; CountFaza < 8; CountFaza++ ) { if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++; } // расчет Фазы с учетор разници Gyro.Frq -=TempFaza*1000; // if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535) // { // Vib.Frq=Vib.Frq-10000; // } // else // { // Vib.Frq=Vib.Frq+1000; // } break; } /* if((CountV & 0x0f) == V1) { ClrV1 ClrV2 }*/ }