Dmitry Kovalev
/
LG2
fork
Fork of LG by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2017-08-16
- Revision:
- 214:4c70e452c491
- Parent:
- 211:ac8251b067d2
- Child:
- 215:b58b887fd367
File content as of revision 214:4c70e452c491:
#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,countFras=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 OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. { static int PeriodCount = 0,Period=0; unsigned int Nmax=0, lowper=1; Gyro.FrqHZ=Gyro.Frq>>16; if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. PeriodCount=0;//сбрасываем таймер sprintf((Time),"%d %d %d\r\n", Gyro.AmpN1, Gyro.AmpN2-Gyro.AmpN1, Gyro.AmpPer); WriteCon(Time); //Gyro.AmpPerDel=(Gyro.AmpPer*100)/Gyro.AmpPerDel; if(Cheng_AMP_Flag==0) { if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);//256 // Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды 63 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 65 Gyro.L_vibro=Gyro.AmpPer*208; sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); WriteCon(Time); Cheng_AMP_Flag=1; } else { if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды if((Gyro.RgConA&0x20)) { Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);//256 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/Gyro.FrqHZ); //левая граница амплитуды 61 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 67 Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*208; sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); WriteCon(Time); } Cheng_AMP_Flag=0; } if(Gyro.AmpN2<(Gyro.AmpN1+2))Gyro.AmpN2=Gyro.AmpN1+2; srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp } else { PeriodCount++;//таймер амплитуды } }*/ void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. { static int PeriodCount = 0,Period=0; unsigned int Nmax=0, lowper=1; Gyro.FrqHZ=Gyro.Frq>>16; if(PeriodCount>= Gyro.AmpT) { PeriodCount=0;//сбрасываем таймер if(Cheng_AMP_Flag==0) { //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды T_vib_1 = Gyro.AmpPer * T_vibP; T_vib_2 = T_vibP * (10000-Gyro.AmpPer); Gyro.L_vibro=Gyro.AmpPer*3; Cheng_AMP_Flag=1; } else { //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды if((Gyro.RgConA&0x20)) { T_vib_1 = T_vibP * (Gyro.AmpPer +(Gyro.AmpPerDel*100)); T_vib_2 = T_vibP * (10000 -(Gyro.AmpPer+(Gyro.AmpPerDel*100))); Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*3; } Cheng_AMP_Flag=0; } srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp } else {PeriodCount++;} } void VibroAMPRegul(void) //подстройка амплитуды ВП { int temp=0; int LowDZ,HiDZ; Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin // Gyro.CaunPlus = Gyro.CaunPlusReper; CaunAddPlus = 0; Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin //Gyro.CaunMin=Gyro.CaunMinReper; CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; if(countFras<512) { countFras++; Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; } else { Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; Gyro.F_ras=Gyro.F_rasAdd>>9; Gyro.F_rasAdd=0; countFras=0; } if(Gyro.RgConA&0x20) { //расчет максимальной амплитуды из востановленного синуса р-р. temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed)); temp=temp>>6; LowDZ = ((Gyro.AmpSpeed<<3)*(-1)); HiDZ = (Gyro.AmpSpeed<<3); 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% } LPC_MCPWM->MAT1 = T_vib_1; LPC_MCPWM->MAT2 = T_vib_2; } void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) { static int TempFaza, CountFaza; TempFaza = -4; Gyro.FrqPhaseEror=0; for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f];} if(Gyro.RgConA&0x40) { //12 Gyro.Frq += TempFaza*Gyro.FrqChengSpeed; } // Gyro.FrqPhaseEror = TempFaza<<10; if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты*/ LPC_TIM1->MR0 =(unsigned int)(103200000/(Gyro.Frq>>11));//запись в таймер нового значение частоты вибро // LPC_TIM1->MR0 =(unsigned int) F_vib; } ////////////////////////////////////////////////////////////////////////////// /////////////////////////основного 32 тактного цикла////////////////////////// ////////////////////////////////////////////////////////////////////////////// void cheng(void) { switch(CountV31) { case 0: BackLightON BackLightON BackLightON ReVib();///обновление значений вибро Gyro.VibroAMPRegulF=1; Time_vibro=0; Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. BackLightOFF BackLightOFF BackLightOFF break; case 8: BackLightON LPC_MCPWM->CON_CLR |= (1<<8); LPC_MCPWM->CON_CLR |= (1<<16); LPC_MCPWM->TC1 =1; LPC_MCPWM->TC2 =1; LPC_MCPWM->CON_SET |= (1<<8); LPC_MCPWM->CON_SET |= (1<<16); //вкл BackLightOFF break; case 16: // Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; // break; case 31: // Gyro.Reper_Event=1; //LPC_MCPWM->LIM0=0; break; } } void AllRegul (void) { ///////////////////////////контуры регулировки///////////////////////////// if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса if (Gyro.VibroAMPRegulF == 1) { Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); if( Gyro.LG_Type==0) { if(!MODFlag)PLCRegul(); } if(Gyro.LG_Type==1) { switch(Gyro.PLCDelay) { case 0:PLCRegul();break; } HFORegul(); } } // Регулеровка Амплитуды виброподвеса if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();} }