123
Fork of LG by
Diff: vibro.c
- Revision:
- 191:40028201ddad
- Parent:
- 190:289514f730ee
- Child:
- 192:d32c8cf7bcd9
diff -r 289514f730ee -r 40028201ddad vibro.c --- a/vibro.c Tue Aug 23 14:03:16 2016 +0000 +++ b/vibro.c Mon Aug 29 11:58:52 2016 +0000 @@ -4,8 +4,8 @@ volatile unsigned int Cheng_AMP_Flag=0; //int reper=0; -int Rate2VibFlag; - +int Rate2VibFlag,countA=0,tempDP; +unsigned int OldMaxAmp=0; void VibroOut(void) // выставка ног вибро { if(CountV31>=16) @@ -33,14 +33,57 @@ } } + + + 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 <20)countA++; + if(countA >5) + { + 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.LogHZ == 1) //Запись для Ориджина. + { + sprintf((Time),"%d \r\n",Gyro.AmpPerDel); + WriteCon(Time); + } + tempDP=Gyro.AmpPerDel; + srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. + Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp + } + + 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);//запись в таймер нового значение частоты вибро + ошибка. + break; + } +}/* +void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. +{ + unsigned int OldMaxAmp=0; + static int PeriodCount = 0; + unsigned int Nmax=0; + Gyro.AmpSC = OldMaxAmp - Gyro.MaxAmp; + OldMaxAmp=Gyro.MaxAmp; + //расчет амплитуды относительно центральной точки - if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. + if(Gyro.MaxAmp >= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. PeriodCount=0;//сбрасываем таймер if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? @@ -78,8 +121,7 @@ LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка. break; } -} - +}*/ void VibroAMPRegul(void) //подстройка амплитуды ВП { Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin @@ -88,19 +130,21 @@ CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р. - Gyro.Amp -= (Gyro.MaxAmp - 3000) * 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% - + 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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100% + // Gyro.MaxAmp =0; } + + void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) { static int TempFaza, CountFaza; TempFaza = -4; - for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} - if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*1000;} + 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.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты @@ -113,19 +157,36 @@ { switch(CountV31) { case 0: - + Gyro.VibroAMPRegulF=1; Time_vibro=0; - Gyro.VibroNoiseF=1; + Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. break; case 16: + /*if(Gyro.LogHZ == 1) //Запись для Ориджина. + { + sprintf((Time),"%d \r\n",Gyro.MaxAmp); + WriteCon(Time); + } */ Time_vibro=0; - Gyro.VibroFrqRegulF=1; + Gyro.VibroFrqRegulF=1; // Gyro.Rate2_Event=1; break; case 24: - Gyro.VibroAMPRegulF=1; + break; } -} \ No newline at end of file +} +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; if(Gyro.ModJump == 3){ShowMod();} else {PlcRegul();}} // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра. + if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса + if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса + if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; CalcAmpN(); } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды + if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно + +}