Dmitry Kovalev
/
LG2
fork
Fork of LG by
Diff: vibro.c
- Revision:
- 209:224e7331a061
- Parent:
- 208:19150d2b528f
- Child:
- 210:b02fa166315d
--- a/vibro.c Tue Feb 07 10:11:35 2017 +0000 +++ b/vibro.c Thu Apr 13 14:14:45 2017 +0000 @@ -4,7 +4,7 @@ volatile unsigned int Cheng_AMP_Flag=0; //int reper=0; int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo; -unsigned int OldMaxAmp=0; +unsigned int OldMaxAmp=0,countFras=0; int z=25; int i=16,tempi=0,klk=0; __irq void EINT3_IRQHandler() @@ -44,250 +44,41 @@ } } -/* -void Calc2AmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. -{ - Gyro.AmpSC=0; - static int PeriodCount = 0; - unsigned int Nmax=0; - Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ; - if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); - OldMaxAmp=Gyro.MaxAmp; - - if(Gyro.AmpSC <55)countA++; - if(countA >2) - { - countA=0; - srand(Global_Time); - if(Cheng_AMP_Flag) - { - Cheng_AMP_Flag=0; - Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - } - else - { - Cheng_AMP_Flag=1; - 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); //правая граница амплитуды - } - } //8046 - - LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро -}*/ -/* -void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. -{ - Gyro.AmpSC=0; - static int PeriodCount = 0; - unsigned int Nmax=0; - Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ; - if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); - OldMaxAmp=Gyro.MaxAmp; - - if(Gyro.AmpSC <5)countA++; - if(countA >3) - { - countA=0; - - Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - Cheng_AMP_Flag=1; - - tempDP=Gyro.AmpPerDel; - srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. - if(Gyro.flag==1) Gyro.AmpPerDel = 1; - else Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - } //8046 - - LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро -} - -*/ - -/* -void CalcAmpD(void) -{ - unsigned int Nmax=0; - countA++; - if( countA>1) - { - countA=0; - - Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - Cheng_AMP_Flag=1; - - // if(Gyro.flag==1) Gyro.AmpPerDel = 1; - - - switch(Znak) { - case 0: - Gyro.AmpPerDel++; - if (Gyro.AmpPerDel>10){Znak=1; fnoize++;} - break; - - case 1: - Gyro.AmpPerDel--; - if (Gyro.AmpPerDel<1)Znak=0; - if (fnoize>6)Znak=2; - break; - - case 2: - Gyro.AmpPerDel++; - if (Gyro.AmpPerDel>7){Znak=3; fnoize++;} - break; - - case 3: - Gyro.AmpPerDel--; - if (Gyro.AmpPerDel<1)Znak=2; - if (fnoize>12){Znak=4;} - break; - - case 4: - Gyro.AmpPerDel++; - if (Gyro.AmpPerDel>15){Znak=5; fnoize++;} - break; - - case 5: - Gyro.AmpPerDel--; - if (Gyro.AmpPerDel<1)Znak=4; - if (fnoize>18){Znak=6;} - break; - - case 6: - Gyro.AmpPerDel++; - if (Gyro.AmpPerDel>6){Znak=7;fnoize++;} - break; - - case 7: - Gyro.AmpPerDel--; - if (Gyro.AmpPerDel<1)Znak=6; - if (fnoize>24){Znak=0;fnoize=0;} - break; - } - } - LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро -} -*/ - -void CalcAmpD(void) -{ - // GyroP.Str.wall++; - // if(GyroP.Str.wall>16) - // { - // GyroP.Str.wall=0; - // klk++; - // if(klk>32) klk = 0; - // } - unsigned int Nmax=0; - Gyro.AmpPerDel = ModArrayTriangle[klk]; - - tempi++; - srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. - Gyro.AmpT = (rand() %8-4);// ОШУМЛЕНИЕ amp - - Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*((100-Gyro.AmpPer)+Gyro.AmpPerDel+Gyro.AmpT))/(Gyro.Frq>>16)); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - - LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро - // LPC_TIM1->MR0 +=(Gyro.AmpT<<5); -} - -/* - -void CalcAmpI(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. -{ - Gyro.AmpSC=0; - static int PeriodCount = 0 ; - unsigned int Nmax=0; - Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ; - if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); - OldMaxAmp=Gyro.MaxAmp; - - countA++; - if( Gyro.AmpSC<60) - { - // countA=0; - - Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); - Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды - Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды - Cheng_AMP_Flag=1; - - // tempDP=Gyro.AmpPerDel; - // srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. - if(Gyro.flag==1) Gyro.AmpPerDel = 1; - - switch(Znak) { - case 0: - Gyro.AmpPerDel--; - if (Gyro.AmpPerDel<1)Znak=1; - break; - - case 1: - Gyro.AmpPerDel++; - if (Gyro.AmpPerDel>13)Znak=0; - break; - } - - } //8046 - LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро -} -*/ - - int Mrand(void) - { - int b=0; - z=z*Gyro.AD_Slow; - b = ((z>>10) & 0xf)+20; - /* sprintf((Time),"%d\r\n", b); - WriteCon(Time);*/ - return b; - } void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. { static int PeriodCount = 0,Period=0; unsigned int Nmax=0, lowper=0; - Gyro.FrqHZ=Gyro.Frq>>16; - //расчет амплитуды относительно центральной точки - if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. - PeriodCount=0;//сбрасываем таймер - - if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? - if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { + Gyro.FrqHZ=Gyro.Frq>>16; + if(PeriodCount>= Gyro.AmpT) + { //если количество заходов в прерывание больше либо равно частоте ошумления. + PeriodCount=0;//сбрасываем таймер + 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) - { + 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; } - // Gyro.AmpN2=Gyro.AmpN1+2; - Cheng_AMP_Flag=1; - // Gyro.L_vibro=(103000/Nmax); - // Gyro.AmpPer=((((((Gyro.Frq>>12)*200)/16)*temp2)/7675000)/2) - Gyro.L_vibro=(((Gyro.AmpPer*7675000)/200)*32)/(Gyro.Frq>>12); - Gyro.L_vibro= Gyro.L_vibro*2; - } + 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)) + 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);//левая граница амплитуды @@ -299,12 +90,16 @@ Gyro.AmpN2= Gyro.AmpN2+lowper+1; } } + // Gyro.AmpN2=Gyro.AmpN1+2; Cheng_AMP_Flag=0; //Gyro.L_vibro=(103000/Nmax); - Gyro.L_vibro=(((Gyro.AmpPer*7675000)/200)*32)/(Gyro.Frq>>12); - Gyro.L_vibro= Gyro.L_vibro*2; + 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; @@ -315,48 +110,51 @@ } else { PeriodCount++;//таймер амплитуды } - } -/*дол лучших времен -unsigned long mwc() -{ -static unsigned long x3456789, -y=362436069, -z=77465321, -c=13579; -unsigned long long t; -tС6905990LL*x+c; -x=y; -y=z; -c=(t>>32); -return z=(t&0xffffffff); -} -*/ - void VibroAMPRegul(void) //подстройка амплитуды ВП { + int temp=0; + Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin + CaunAddPlus = 0; - Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin + Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin + CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; - Gyro.F_ras = Gyro.MaxAmp/32*Gyro.FrqHZ/22.5; + + + if(countFras<512) + { + countFras++; + Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/22.5; + } + else + { + + Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/22; + Gyro.F_ras=Gyro.F_rasAdd>>9; + Gyro.F_rasAdd=0; + countFras=0; + } + + if(Gyro.RgConA&0x20) { - //расчет максимальной амплитуды из востановленного синуса р-р. + //расчет максимальной амплитуды из востановленного синуса р-р. temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed)); temp=temp>>5; 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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + 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% } - /* sprintf((Time),"%d \r\n", ( Gyro.F_ras)); - WriteCon(Time);*/ + + } @@ -365,11 +163,14 @@ { static int TempFaza, CountFaza; TempFaza = -4; + Gyro.FrqPhaseEror=0; + for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0 + for (CountFaza = 0; CountFaza <8; CountFaza++ ) {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f];} if(Gyro.RgConA&0x40) { //12 - for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0 - Gyro.Frq += TempFaza*Gyro.FrqChengSpeed; + Gyro.Frq += TempFaza*Gyro.FrqChengSpeed; } + // Gyro.FrqPhaseEror = TempFaza<<10; if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро @@ -388,7 +189,7 @@ break; case 16: - Gyro.Reper_Event=1; + // Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; // break; @@ -414,8 +215,14 @@ if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса - if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); PLCRegul(); } // Регулеровка Амплитуды виброподвеса - if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; /*CalcAmpD();*/ OLDCalcAmpN();} + if (Gyro.VibroAMPRegulF == 1) + { + Gyro.VibroAMPRegulF = 0; + VibroAMPRegul(); + if(!MODFlag)PLCRegul(); + + } // Регулеровка Амплитуды виброподвеса + if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();} /* { switch(Gyro.flag) { case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break;