Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
Diff: vibro.c
- Revision:
- 112:4a96133a1311
- Parent:
- 108:030cdde08314
- Child:
- 113:8be429494918
--- a/vibro.c Mon Apr 04 03:21:25 2016 +0000 +++ b/vibro.c Mon Apr 04 11:09:56 2016 +0000 @@ -1,12 +1,8 @@ #include "Global.h" GyroT Gyro; -//volatile int V1 = 0 ; -//volatile int Temp = 0 ; -volatile unsigned int Flag=0; -unsigned int FrecTemp=0; -unsigned int FrecTp=0,amp=0; +volatile unsigned int Cheng_AMP_Flag=0; void Discharg ()//проверка битового поля поджига и установка значения бита поджига { if (Gyro.Discharg) @@ -48,53 +44,68 @@ Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1" } } - - - } void CalcAmpN(void) { + static int PeriodCount = 0; unsigned int Nmax=0; //расчет амплитуды относительно центральной точки - if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. - PeriodCount=0; + PeriodCount=0;//сбрасываем таймер - if (Flag==0) { //сейчас малая амплитуда? + if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { - Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды + Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды } 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; + Cheng_AMP_Flag=1; } else { if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { - Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници мплитуды + 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; + Cheng_AMP_Flag=0; } - - //Tnoise=55; srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. - Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - //изменение флага предидущей амплитуды - + } else { - PeriodCount++; + PeriodCount++;//таймер амплитуды } + +} + +void VibroAMPRegul(void) //подстройка амплитуды ВП +{ + Gyro.CaunPlus = CaunAddPlus; + CaunAddPlus = 0; + Gyro.CaunMin = CaunAddMin; + CaunAddMin = 0; + + Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;//расчет максимальной амплитуды из востановленного синуса р-р. + Gyro.Amp -= (Gyro.MaxAmp - 5000) * 1; // расчет амплитуды ВП с учетом разници + if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100% +} + +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&0x1) {Gyro.Frq += TempFaza*1000;} /// перепутан вибро 1 вибро 2 } ////////////////////////////////////////////////////////////////////////////// @@ -102,52 +113,21 @@ ////////////////////////////////////////////////////////////////////////////// void cheng(void) { - static int TempFaza, CountFaza; - -// if (Buff_Restored_sin [CountV31] > MaxAmp) {MaxAmp = Buff_Restored_sin [CountV31];} // подумать со знаком*/ - - switch(CountV31) { - case 0: - CalcAmpN();//расчет ошумления и амплитуды задание интервалов выставки ножек вибро. - //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; //прием частоты из аск_глд старый протокол + case 0: + Time_vibro=0; + Gyro.VibroNoiseF=1; LPC_TIM1->MR0 =(unsigned int)(100000000/(Gyro.Frq>>11));//запись в таймер 1 значение частоты вибро -// Output.Str.T_Vibro=(unsigned int)((7680000*16/Gyro.Frq)*4096); //запись частоты для выдачи в аск_глд старый протокол - Time_vibro=0; - break; - case 24: -MaxAmp = CaunAddPlus + CaunAddMin; -CaunPlus = CaunAddPlus; -CaunMin = CaunAddMin; -CaunAddPlus = 0; -CaunAddMin = 0; - - Gyro.Amp -= (MaxAmp - 5000) * 1; // расчет амплитудв с учетом разници - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16); // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// - } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - Gyro.AmpPer = Gyro.Amp>>16; - + break; + + case 16: + Time_vibro=0; + Gyro.VibroFrqRegulF=1; + break; -// amp=MaxAmp; -// MaxAmp=0; - break; - - case 16: - Time_vibro=0; - -// расчет Фазы с учетор разници - TempFaza = -4; - for (CountFaza = 0; CountFaza <8; CountFaza++ ) { - if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++; - } - // FrecTp=Buff_Restored_sin [(CountV31 - 12 + CountFaza) & 0xff]; - if(Gyro.RgConA&0x1){ - Gyro.Frq += TempFaza*1000;/// перепутан вибро 1 вибро 2 - } - break; + case 24: + Gyro.VibroAMPRegulF=1; + break; } - } \ No newline at end of file