Dmitry Kovalev
/
LG2
fork
Fork of LG by
Diff: vibro.c
- Revision:
- 208:19150d2b528f
- Parent:
- 207:d1ce992f5d17
- Child:
- 209:224e7331a061
--- a/vibro.c Fri Jan 13 08:27:28 2017 +0000 +++ b/vibro.c Tue Feb 07 10:11:35 2017 +0000 @@ -6,9 +6,10 @@ int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo; unsigned int OldMaxAmp=0; int z=25; -int i=16,tempi=0; +int i=16,tempi=0,klk=0; __irq void EINT3_IRQHandler() -{ Gyro.EXT_Latch=1; +{ + Gyro.EXT_Latch=1; // Gyro.DeltaEXT_Event=1; // Gyro.B_Delta_EventEXT=1; LPC_GPIOINT->IO0IntClr |= (1<<1); @@ -176,19 +177,27 @@ */ void CalcAmpD(void) -{ - i++; - unsigned int Nmax=0; - Gyro.AmpPerDel = ModArrayTriangle[i]; +{ + // 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() % 2);// ОШУМЛЕНИЕ amp + 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); //правая граница амплитуды - if(i>32) i = 0; LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро + // LPC_TIM1->MR0 +=(Gyro.AmpT<<5); } /* @@ -247,7 +256,7 @@ { static int PeriodCount = 0,Period=0; unsigned int Nmax=0, lowper=0; - + Gyro.FrqHZ=Gyro.Frq>>16; //расчет амплитуды относительно центральной точки if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. PeriodCount=0;//сбрасываем таймер @@ -259,7 +268,7 @@ 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<20) + if(Gyro.AmpPer<5) { lowper = Gyro.AmpN2-Gyro.AmpN1; lowper=lowper/2; @@ -267,26 +276,34 @@ } // 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; } - else { + 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<20) - { + 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=(((Gyro.AmpPer*7675000)/200)*32)/(Gyro.Frq>>12); + Gyro.L_vibro= Gyro.L_vibro*2; } // Period=Gyro.CuruAngle*101; @@ -299,7 +316,7 @@ PeriodCount++;//таймер амплитуды } -LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро + } /*дол лучших времен @@ -319,20 +336,28 @@ */ void VibroAMPRegul(void) //подстройка амплитуды ВП -{ +{ + int temp=0; Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddPlus = 0; Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddMin = 0; - Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р. - Gyro.Amp -=(int)(((Gyro.MaxAmp - Gyro.AmpTarget) * 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% - /* sprintf((Time),"%d %d %d %d\r\n",Gyro.MaxAmp,(int)((((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed)>>1)), Gyro.Amp>>16, Gyro.Amp); - WriteCon(Time);*/ - -} + Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; + Gyro.F_ras = Gyro.MaxAmp/32*Gyro.FrqHZ/22.5; + 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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100% + } + + /* sprintf((Time),"%d \r\n", ( Gyro.F_ras)); + WriteCon(Time);*/ + } @@ -340,11 +365,14 @@ { static int TempFaza, CountFaza; TempFaza = -4; - for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0 - if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;} - + 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; + } 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));//запись в таймер нового значение частоты вибро } ////////////////////////////////////////////////////////////////////////////// @@ -354,32 +382,15 @@ { switch(CountV31) { case 0: - /* tempi++; - if( tempi>31) - {*/ - sprintf((Time),"%d %d %d %d %d\r\n", (Gyro.AmpPerDel+Gyro.AmpT)*50,Gyro.CuruAngle32*500,(Buff_32Point[CountV31])*500, CaunAddPlus-3200, CaunAddMin-3200); - WriteCon(Time); - Gyro.CuruAngle32=0; - tempi=0; -/* } - - sprintf((Time)," %d %d\r\n",(Gyro.AmpPerDel+Gyro.AmpT),0); - WriteCon(Time);*/ - Gyro.VibroAMPRegulF=1; Time_vibro=0; Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. - /* sprintf((Time),"%d\r\n",Gyro.AmpT); - WriteCon(Time);*/ break; case 16: - Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; // - /*sprintf((Time),"%d\r\n",Gyro.AmpT); - WriteCon(Time);*/ break; } } @@ -403,8 +414,8 @@ if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса - if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса - if (Gyro.VibroNoiseF == 4) {Gyro.VibroNoiseF = 0; CalcAmpD();/*OLDCalcAmpN();*/} + if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); PLCRegul(); } // Регулеровка Амплитуды виброподвеса + if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; /*CalcAmpD();*/ OLDCalcAmpN();} /* { switch(Gyro.flag) { case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break;