Dmitry Kovalev
/
LG2
fork
Fork of LG by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2016-09-01
- Revision:
- 192:d32c8cf7bcd9
- Parent:
- 191:40028201ddad
- Child:
- 193:a0fe8bfc97e4
File content as of revision 192:d32c8cf7bcd9:
#include "Global.h" GyroT Gyro; GyroParam GyroP; volatile unsigned int Cheng_AMP_Flag=0; //int reper=0; int Rate2VibFlag,countA=0,tempDP; unsigned int OldMaxAmp=0; void VibroOut(void) // выставка ног вибро { if(CountV31>=16) {//первая нога вибро // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) { Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "0" } else { Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1" } } else {//вторая нога вибро if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) { Gyro.PinReg &= ~PinRegBit_2V; //установить в регистре PinReg бит "вибро 2" в "0" } else { Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1" } } } 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() для получения новых случайных велечин. if(Gyro.flag==1) Gyro.AmpPerDel = 1; else 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)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. { 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);//запись в таймер нового значение частоты вибро + ошибка. break; } } void VibroAMPRegul(void) //подстройка амплитуды ВП { Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddPlus = 0; 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);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// 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++;} //резонанс когда 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;//верхнее ограничение частоты } ////////////////////////////////////////////////////////////////////////////// /////////////////////////основного 32 тактного цикла////////////////////////// ////////////////////////////////////////////////////////////////////////////// void cheng(void) { switch(CountV31) { case 0: 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); } */ Time_vibro=0; Gyro.VibroFrqRegulF=1; // Gyro.Rate2_Event=1; break; case 24: 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; 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) { if(Gyro.flag==2) { Gyro.AmpPerDel=2; Gyro.AmpMin=20000; Gyro.AmpTD=100; OLDCalcAmpN(); } else { Gyro.AmpMin =2; Gyro.AmpTD =6; Gyro.VibroNoiseF = 0; CalcAmpN(); } } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно }