Dmitry Kovalev
fork
Diff: vibro.c
- Revision:
- 193:a0fe8bfc97e4
- Parent:
- 192:d32c8cf7bcd9
- Child:
- 194:8f3cb37a5541
--- a/vibro.c Thu Sep 01 08:23:30 2016 +0000
+++ b/vibro.c Thu Sep 15 11:09:00 2016 +0000
@@ -3,8 +3,19 @@
GyroParam GyroP;
volatile unsigned int Cheng_AMP_Flag=0;
//int reper=0;
-int Rate2VibFlag,countA=0,tempDP;
+int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo;
unsigned int OldMaxAmp=0;
+
+
+__irq void EINT3_IRQHandler()
+{
+ Gyro.DeltaEXT_Event=1;
+ Gyro.B_Delta_EventEXT=1;
+ LPC_GPIOINT->IO0IntClr |= (1<<1);
+ // CMD_Delta_Bins();
+}
+
+
void VibroOut(void) // выставка ног вибро
{
if(CountV31>=16)
@@ -33,7 +44,39 @@
}
-
+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)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
{
@@ -45,7 +88,7 @@
OldMaxAmp=Gyro.MaxAmp;
if(Gyro.AmpSC <20)countA++;
- if(countA >5)
+ if(countA >3)
{
countA=0;
@@ -53,69 +96,180 @@
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.LogHZ == 1) //Запись для Ориджина.
- {
- sprintf((Time),"%d \r\n",Gyro.AmpPerDel);
- WriteCon(Time);
- }
+
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;/*fnoize=0;*/}
+ 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;*/}
+ 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 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));//запись в таймер нового значение частоты вибро
+}
+
+
+
+void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
+{
+ static int PeriodCount = 0;
+ unsigned int Nmax=0;
+
+ //расчет амплитуды относительно центральной точки
+ 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)((100000/(Gyro.Frq>>16))-1); //
+ Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды
+ Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
+ Cheng_AMP_Flag=1;
+
+ }
+
+ else {
+ if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
+ Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
+ }
+
+ Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
+ Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды
+ Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
+ Cheng_AMP_Flag=0;
+
+ }
+ srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
+ Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
+
+ } else {
+ PeriodCount++;//таймер амплитуды
+ }
switch( Gyro.StrayHZ_flag) {
- case 0: //8046
- LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
+ case 0:
+ LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
break;
case 1:
- LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
- break;
- }
-}
-void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
-{
- static int PeriodCount;
- unsigned int Nmax;
-
- //расчет амплитуды относительно центральной точки
- if(PeriodCount >= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
- PeriodCount=0;//сбрасываем таймер
-
- if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
- /* if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды
- else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды
- */
- 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); //правая граница амплитуды
-
- Cheng_AMP_Flag=1;
- }
-
- else {
- /* if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды
- else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды*/
-
- 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=0;
- }
- srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
- Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
- }
- else {PeriodCount++;}
-
- switch( Gyro.StrayHZ_flag) {
- case 0: //8046
- LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
- break;
-
- case 1:
- LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
+ LPC_TIM1->MR0 =(unsigned int)((100000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
break;
}
}
@@ -125,8 +279,8 @@
CaunAddPlus = 0;
Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
CaunAddMin = 0;
-
- Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р.
+ /*Gyro.tempdelta=Gyro.CaunPlus - Gyro.CaunMin ;*/
+ 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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
@@ -154,25 +308,20 @@
{
switch(CountV31) {
case 0:
+ //CuruAngle();//СЧЕТ УГЛА ПО 32 ТОЧКАМ.
Gyro.VibroAMPRegulF=1;
Time_vibro=0;
Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
break;
-
case 16:
- /*if(Gyro.LogHZ == 1) //Запись для Ориджина.
- {
- sprintf((Time),"%d \r\n",Gyro.MaxAmp);
- WriteCon(Time);
- } */
+ Gyro.Reper_Event=1;
Time_vibro=0;
Gyro.VibroFrqRegulF=1; //
- Gyro.Rate2_Event=1;
break;
- case 24:
-
- break;
+ case 31:
+ //CuruAngleLOG();
+ break;
}
}
void AllRegul (void)
@@ -185,19 +334,32 @@
if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса
if (Gyro.VibroNoiseF == 1)
{
+
if(Gyro.flag==2)
{
- Gyro.AmpPerDel=2;
- Gyro.AmpMin=20000;
- Gyro.AmpTD=100;
- OLDCalcAmpN();
+ //Gyro.AmpPer=20;
+ Gyro.AmpPerDel=10;
+ Gyro.AmpMin =10;// минимальное значение AmpT;
+ Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления)
+ OLDCalcAmpN();
+ Gyro.VibroNoiseF = 0;
}
+ else if(Gyro.flag==3)
+ {
+ Gyro.AmpMin =3;
+ Gyro.AmpTD =10;
+ Gyro.VibroNoiseF = 0;
+ Calc2AmpN();
+ }
else
{
- Gyro.AmpMin =2;
- Gyro.AmpTD =6;
+ Gyro.AmpMin =1;
+ Gyro.AmpTD =10;
Gyro.VibroNoiseF = 0;
- CalcAmpN();
+ //CalcAmpN();
+ Gyro.AmpTarget =15000;
+ CalcAmpI();
+ //CalcAmpD();
}
} // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно