Dmitry Kovalev
fork
Diff: vibro.c
- Revision:
- 196:f76dbc081e63
- Parent:
- 194:8f3cb37a5541
- Child:
- 197:7a05523bf588
--- 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(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно