Dmitry Kovalev
forkd
Fork of
LGstaandart
by 
Dmitry Kovalev
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;