Dmitry Kovalev
fork
Diff: vibro.c
- Revision:
- 207:d1ce992f5d17
- Parent:
- 205:775d54fdf646
- Child:
- 208:19150d2b528f
--- 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;