![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
forkd
Fork of LG2 by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2017-02-07
- Revision:
- 208:19150d2b528f
- Parent:
- 207:d1ce992f5d17
- Child:
- 209:224e7331a061
File content as of revision 208:19150d2b528f:
#include "Global.h" GyroT Gyro; GyroParam GyroP; 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; int z=25; int i=16,tempi=0,klk=0; __irq void EINT3_IRQHandler() { Gyro.EXT_Latch=1; // Gyro.DeltaEXT_Event=1; // Gyro.B_Delta_EventEXT=1; LPC_GPIOINT->IO0IntClr |= (1<<1); // CMD_Delta_Bins(); } void VibroOut(void) // выставка ног вибро { if(CountV31>=16) {//первая нога вибро // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { SetV1//установить в регистре PinReg бит "вибро 1" в "0" } else { ClrV1 //установить в регистре PinReg бит "вибро 1" в "1" } } else {//вторая нога вибро if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { SetV2 //установить в регистре PinReg бит "вибро 2" в "0" } else { ClrV2//установить в регистре PinReg бит "вибро 2" в "1" } } } /* 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.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) { 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; } 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<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; srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. // srand(Mrand()); Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp } 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 CaunAddMin = 0; 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);*/ } void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) { static int TempFaza, CountFaza; TempFaza = -4; 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));//запись в таймер нового значение частоты вибро } ////////////////////////////////////////////////////////////////////////////// /////////////////////////основного 32 тактного цикла////////////////////////// ////////////////////////////////////////////////////////////////////////////// void cheng(void) { switch(CountV31) { case 0: Gyro.VibroAMPRegulF=1; Time_vibro=0; Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. break; case 16: Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; // break; } } void AllRegul (void) { ///////////////////////////контуры регулировки///////////////////////////// if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD /*if (Gyro.ADS_NewData == 1) { Gyro.ADS_NewData = 0; switch(Gyro.LogPLC) { case 0: PlcRegul(); break; case 1: PlcRegul(); break; case 2: ShowMod(); break; } }*/// был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра. 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();} /* { switch(Gyro.flag) { case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break; case 2: Gyro.AmpMin =3;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;Calc2AmpN(); break; case 3: Gyro.AmpMin =1;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;CalcAmpD(); break; } } */// регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды //if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut();} // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно }