Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
Diff: vibro.c
- Revision:
- 210:b02fa166315d
- Parent:
- 209:224e7331a061
- Child:
- 211:ac8251b067d2
--- a/vibro.c Thu Apr 13 14:14:45 2017 +0000 +++ b/vibro.c Mon Apr 24 05:43:54 2017 +0000 @@ -48,65 +48,48 @@ void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. { static int PeriodCount = 0,Period=0; - unsigned int Nmax=0, lowper=0; + unsigned int Nmax=0, lowper=1; Gyro.FrqHZ=Gyro.Frq>>16; if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. PeriodCount=0;//сбрасываем таймер + /* sprintf((Time),"%d %d %d\r\n", Gyro.AmpN1, Gyro.AmpN2-Gyro.AmpN1, Gyro.AmpPer); + WriteCon(Time);*/ + if(Cheng_AMP_Flag==0) { if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды - } - Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1); // - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - if(Gyro.AmpPer<5) - { - lowper = Gyro.AmpN2-Gyro.AmpN1; - lowper=lowper/2; - Gyro.AmpN2= Gyro.AmpN2+lowper; - } + } //26214400 + Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);//256 // + Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды 63 + Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 65 + Gyro.L_vibro=Gyro.AmpPer*208; + /* sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); + WriteCon(Time);*/ Cheng_AMP_Flag=1; - Gyro.L_vibro=(((16383 *(Gyro.AmpN2-Gyro.AmpN1))/(Nmax/2))); - // Gyro.L_vibro= Gyro.L_vibro/*2*/; } - - else - { - if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { - Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды - } - if(/*(Gyro.RgConA&0x40)&&*/(Gyro.RgConA&0x20)) - { - Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - if(Gyro.AmpPer<5) - { - lowper = Gyro.AmpN2-Gyro.AmpN1; - lowper=lowper/2; - Gyro.AmpN2= Gyro.AmpN2+lowper+1; - } - } - - // Gyro.AmpN2=Gyro.AmpN1+2; - Cheng_AMP_Flag=0; - //Gyro.L_vibro=(103000/Nmax); - Gyro.L_vibro=(((16383 *(Gyro.AmpN2-Gyro.AmpN1))/(Nmax/2))); - // Gyro.L_vibro=(((Gyro.AmpPer*7680000)/25)*8)/(Gyro.Frq>>12); - // Gyro.L_vibro= Gyro.L_vibro/*2*/; - } - /* sprintf((Time)," %d %d\r\n",(16383 *((Gyro.AmpN2-Gyro.AmpN1)/Nmax)), Gyro.L_vibro); - WriteCon(Time);*/ - - // Period=Gyro.CuruAngle*101; - + else + { + if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды + if((Gyro.RgConA&0x20)) + { + Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);//256 + Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/Gyro.FrqHZ); //левая граница амплитуды 61 + Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 67 + Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*208; + /*sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); + WriteCon(Time); */ + } + Cheng_AMP_Flag=0; + } + if(Gyro.AmpN2<(Gyro.AmpN1+2))Gyro.AmpN2=Gyro.AmpN1+2; + srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. - // srand(Mrand()); - Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - + Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp + // Gyro.L_vibro= ((Gyro.AmpN2-Gyro.AmpN1))*(16383/Nmax); + // Gyro.L_vibro=((16000 *(Gyro.AmpN2-Gyro.AmpN1))/(Nmax/2)); } else { PeriodCount++;//таймер амплитуды } @@ -119,10 +102,10 @@ int temp=0; Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin - + // Gyro.CaunPlus = Gyro.CaunPlusReper; CaunAddPlus = 0; Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin - + //Gyro.CaunMin=Gyro.CaunMinReper; CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; @@ -130,13 +113,14 @@ if(countFras<512) { countFras++; - Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/22.5; + Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; } else - { - - Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/22; - Gyro.F_ras=Gyro.F_rasAdd>>9; + { + sprintf((Time)," %d %d %d\r\n",Gyro.F_ras, Gyro.MaxAmp, Gyro.AmpTarget/((Gyro.Frq)>>16)); + WriteCon(Time); + Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; + Gyro.F_ras=Gyro.F_rasAdd>>8; Gyro.F_rasAdd=0; countFras=0; } @@ -145,16 +129,13 @@ if(Gyro.RgConA&0x20) { //расчет максимальной амплитуды из востановленного синуса р-р. - temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed)); - temp=temp>>5; + temp=(int)(((Gyro.MaxAmp -Gyro.AmpTarget/((Gyro.Frq)>>16)) * Gyro.AmpSpeed)); + temp=temp>>6; Gyro.Amp -= temp; // расчет амплитуды ВП с учетом разници if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100% } - - - }