Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
Diff: vibro.c
- Revision:
- 189:8a16378724c4
- Parent:
- 188:4c523cc373cc
- Child:
- 190:289514f730ee
diff -r 4c523cc373cc -r 8a16378724c4 vibro.c --- a/vibro.c Fri Aug 19 14:02:02 2016 +0000 +++ b/vibro.c Mon Aug 22 10:39:08 2016 +0000 @@ -25,9 +25,6 @@ Gyro.BackLight = Gyro.BackLight >> 1; } } - - - void VibroOut(void) // выставка ног вибро { if(CountV31>=16) {//первая нога вибро @@ -55,40 +52,33 @@ //расчет амплитуды относительно центральной точки if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. - PeriodCount=0;//сбрасываем таймер + PeriodCount=0;//сбрасываем таймер + if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? - if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { - Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды - } - else if((Gyro.AmpPer+Gyro.AmpPerDel)<18) { - Gyro.AmpPer=18+Gyro.AmpPerDel; //проверка нижней граници амплитуды - } - // 239 - Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); + if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды + else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды + + Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды + Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды + Cheng_AMP_Flag=1; } else { - if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { - Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды - } - else if((Gyro.AmpPer+Gyro.AmpPerDel)<18) { - Gyro.AmpPer=18+Gyro.AmpPerDel; //проверка нижней граници амплитуды - } - Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); + if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды + else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды + + Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16));//левая граница амплитуды - // Gyro.AmpN1=(unsigned int)((Nmax*(100-(Gyro.AmpPer-Gyro.AmpPerDel)))/(Gyro.Frq>>16));//левая граница амплитуды Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды + Cheng_AMP_Flag=0; } srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - - } else { - PeriodCount++;//таймер амплитуды 26214400 - } + } + else {PeriodCount++;} switch( Gyro.StrayHZ_flag) { case 0: //8046 @@ -103,17 +93,18 @@ void VibroAMPRegul(void) //подстройка амплитуды ВП { - Gyro.CaunPlus = CaunAddPlus; + Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddPlus = 0; - Gyro.CaunMin = CaunAddMin; + Gyro.CaunMin = CaunAddMin;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;//расчет максимальной амплитуды из востановленного синуса р-р. - Gyro.Amp -= (Gyro.MaxAmp - 4000) * 1; // расчет амплитуды ВП с учетом разници + 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% - if((Gyro.Amp>>16) > 95) {Gyro.Amp= (95 << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// - - if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100% } void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) @@ -122,6 +113,9 @@ 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;} + + if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты + else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты } ////////////////////////////////////////////////////////////////////////////// @@ -129,11 +123,6 @@ ////////////////////////////////////////////////////////////////////////////// void cheng(void) { - - /* -if (CountV255 == 0) Gyro.Rate2_Event=1; -if (CountV255 == 127) Gyro.Rate2_Event=1; -*/ switch(CountV31) { case 0: