Dmitry Kovalev
fork
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;