Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
Diff: vibro.c
- Revision:
- 196:f76dbc081e63
- Parent:
- 194:8f3cb37a5541
- Child:
- 197:7a05523bf588
diff -r bcc769f5292b -r f76dbc081e63 vibro.c --- a/vibro.c Tue Sep 20 10:39:30 2016 +0000 +++ b/vibro.c Fri Sep 23 05:34:50 2016 +0000 @@ -87,7 +87,7 @@ if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); OldMaxAmp=Gyro.MaxAmp; - if(Gyro.AmpSC <20)countA++; + if(Gyro.AmpSC <5)countA++; if(countA >3) { countA=0; @@ -135,9 +135,7 @@ if (Gyro.AmpPerDel<1)Znak=0; if (fnoize>6)Znak=2; break; - - - + case 2: Gyro.AmpPerDel++; if (Gyro.AmpPerDel>7){Znak=3; fnoize++;} @@ -147,11 +145,7 @@ if (Gyro.AmpPerDel<1)Znak=2; if (fnoize>12){Znak=4;/*fnoize=0;*/} break; - - - - - + case 4: Gyro.AmpPerDel++; if (Gyro.AmpPerDel>15){Znak=5; fnoize++;} @@ -161,12 +155,7 @@ if (Gyro.AmpPerDel<1)Znak=4; if (fnoize>18){Znak=6;/*fnoize=0;*/} break; - - - - - - + case 6: Gyro.AmpPerDel++; if (Gyro.AmpPerDel>6){Znak=7;fnoize++;} @@ -263,15 +252,11 @@ } else { PeriodCount++;//таймер амплитуды } - switch( Gyro.StrayHZ_flag) { - case 0: - LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро - break; - case 1: - LPC_TIM1->MR0 =(unsigned int)((100000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка. - break; - } +LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро + + + } void VibroAMPRegul(void) //подстройка амплитуды ВП { @@ -283,15 +268,16 @@ 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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100% // Gyro.MaxAmp =0; + } void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) -{ +{ 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 @@ -308,19 +294,34 @@ { switch(CountV31) { case 0: - //CuruAngle();//СЧЕТ УГЛА ПО 32 ТОЧКАМ. - Gyro.VibroAMPRegulF=1; + Gyro.VibroAMPRegulF=1; Time_vibro=0; Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. + break; + case 16: //Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; // break; - - case 31: - //CuruAngleLOG(); + + 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.LogHZ) + { + sprintf((Time),"%d %d %d\r\n", Gyro.CuruAngle,Gyro.tempdelta,Gyro.tempdelta2); WriteCon(Time); + Gyro.CuruAngle=0; + Gyro.tempdelta=0; + Gyro.tempdelta2=0; + + } + break; } } @@ -329,38 +330,48 @@ if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD - if (Gyro.ADS_NewData == 1) {Gyro.ADS_NewData = 0; if(Gyro.ModJump == 3){ShowMod();} else {PlcRegul();}} // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра. + if (Gyro.ADS_NewData == 1) + { + Gyro.ADS_NewData = 0; + switch(Gyro.LogPLC) { + case 0: PlcRegul(); break; + case 1: PlcRegul(); break; + case 2: ShowMod(); break; + } + } // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра. if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса if (Gyro.VibroNoiseF == 1) { - - if(Gyro.flag==2) - { - //Gyro.AmpPer=20; - Gyro.AmpPerDel=10; + /*Gyro.AmpPerDel=10; Gyro.AmpMin =10;// минимальное значение AmpT; Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления) + Gyro.VibroNoiseF = 0; + OLDCalcAmpN();*/ + + switch(Gyro.flag) { + case 1: + Gyro.VibroNoiseF = 0; OLDCalcAmpN(); - Gyro.VibroNoiseF = 0; - } - else if(Gyro.flag==3) - { - Gyro.AmpMin =3; + break; + + case 2: + Gyro.AmpMin =3; Gyro.AmpTD =10; Gyro.VibroNoiseF = 0; - Calc2AmpN(); - } - else - { + Calc2AmpN(); + break; + + case 3: Gyro.AmpMin =1; Gyro.AmpTD =10; Gyro.VibroNoiseF = 0; //CalcAmpN(); Gyro.AmpTarget =15000; - CalcAmpI(); - //CalcAmpD(); - } + //CalcAmpI(); + CalcAmpD(); + break; +} } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно