Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
Diff: vibro.c
- Revision:
- 207:d1ce992f5d17
- Parent:
- 205:775d54fdf646
- Child:
- 208:19150d2b528f
diff -r 00341a03e05c -r d1ce992f5d17 vibro.c --- a/vibro.c Tue Dec 20 14:08:17 2016 +0000 +++ b/vibro.c Fri Jan 13 08:27:28 2017 +0000 @@ -5,8 +5,8 @@ //int reper=0; 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; __irq void EINT3_IRQHandler() { Gyro.EXT_Latch=1; // Gyro.DeltaEXT_Event=1; @@ -27,14 +27,14 @@ } else { - ClrV1 //установить в регистре PinReg бит "вибро 1" в "1" + ClrV1 //установить в регистре PinReg бит "вибро 1" в "1" } } else {//вторая нога вибро if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { - SetV2 //установить в регистре PinReg бит "вибро 2" в "0" + SetV2 //установить в регистре PinReg бит "вибро 2" в "0" } else { @@ -108,7 +108,7 @@ */ - +/* void CalcAmpD(void) { unsigned int Nmax=0; @@ -130,6 +130,7 @@ Gyro.AmpPerDel++; if (Gyro.AmpPerDel>10){Znak=1; fnoize++;} break; + case 1: Gyro.AmpPerDel--; if (Gyro.AmpPerDel<1)Znak=0; @@ -139,27 +140,30 @@ case 2: Gyro.AmpPerDel++; if (Gyro.AmpPerDel>7){Znak=3; fnoize++;} - break; + break; + case 3: Gyro.AmpPerDel--; if (Gyro.AmpPerDel<1)Znak=2; - if (fnoize>12){Znak=4;/*fnoize=0;*/} + 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;/*fnoize=0;*/} + 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; @@ -169,8 +173,23 @@ } LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро } +*/ +void CalcAmpD(void) +{ + i++; + unsigned int Nmax=0; + Gyro.AmpPerDel = ModArrayTriangle[i]; + srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. + Gyro.AmpT = (rand() % 2);// ОШУМЛЕНИЕ 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));//запись в таймер нового значение частоты вибро +} /* @@ -214,24 +233,33 @@ } */ + 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; +{ + static int PeriodCount = 0,Period=0; unsigned int Nmax=0, lowper=0; //расчет амплитуды относительно центральной точки if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. PeriodCount=0;//сбрасываем таймер - if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? + if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды } - Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); // + 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<6) + if(Gyro.AmpPer<20) { lowper = Gyro.AmpN2-Gyro.AmpN1; lowper=lowper/2; @@ -247,10 +275,10 @@ Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды } - Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); + 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<6) + if(Gyro.AmpPer<20) { lowper = Gyro.AmpN2-Gyro.AmpN1; lowper=lowper/2; @@ -259,18 +287,37 @@ // Gyro.AmpN2=Gyro.AmpN1+2; Cheng_AMP_Flag=0; - } - /* sprintf((Time),"%d %d %d %d\r\n", Gyro.AmpPer, Nmax, Gyro.AmpN1, Gyro.AmpN2 );//выдаем в терминал для постройки граффика регулировки периметра. - WriteCon(Time);*/ - srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. - Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp - + } + + // Period=Gyro.CuruAngle*101; + + srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. + // srand(Mrand()); + Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp + } else { PeriodCount++;//таймер амплитуды } -LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро +LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро } + +/*дол лучших времен +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) //подстройка амплитуды ВП { Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin @@ -278,11 +325,12 @@ Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р. - Gyro.Amp -= (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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + 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);*/ } @@ -306,11 +354,23 @@ { 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=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. - + Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. + /* sprintf((Time),"%d\r\n",Gyro.AmpT); + WriteCon(Time);*/ break; case 16: @@ -318,39 +378,10 @@ Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; // + /*sprintf((Time),"%d\r\n",Gyro.AmpT); + WriteCon(Time);*/ break; - - case 31: - /* for (int i = 0; i < 32; i++ ) - { - Gyro.CuruAngle += Buff_32Point [i]; - Gyro.tempdelta += (Buff_Restored_sin[i]); - Gyro.tempdelta2 += (Buff_Restored_sin2[i]) - } - */ - if(Gyro.Log) - { - sprintf((Time),"%d %d %d\r\n",ADCDIF,(ADCDIF>>5),(ADCDIF>>10)); - WriteCon(Time); - ADCDIF=0; - Gyro.CuruAngle=0; - - } /* - if(Gyro.ModJump==2) { ///прыжок с моды на моду. (-->) - Gyro.ModJump=0; - Spi.DAC_B += 4300; - Gyro.PLC_Error2Mode=1; - } - - if(Gyro.ModJump==1) { ///прыжок с моды на моду. (<--) - Gyro.OldCuruAngle=Gyro.CuruAngle; - Gyro.ModJump=0; - Spi.DAC_B -= 5900; - Gyro.PLC_Error2Mode=1; - } - - break;*/ - } + } } void AllRegul (void) { ///////////////////////////контуры регулировки///////////////////////////// @@ -373,7 +404,7 @@ if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса - if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();} + if (Gyro.VibroNoiseF == 4) {Gyro.VibroNoiseF = 0; CalcAmpD();/*OLDCalcAmpN();*/} /* { switch(Gyro.flag) { case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break;