123
Fork of LG by
Diff: vibro.c
- Revision:
- 193:a0fe8bfc97e4
- Parent:
- 192:d32c8cf7bcd9
- Child:
- 194:8f3cb37a5541
diff -r d32c8cf7bcd9 -r a0fe8bfc97e4 vibro.c --- 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(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно