![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
forkd
Fork of LGstaandart by
vibro.c
- Committer:
- Kovalev_D
- Date:
- 2017-12-26
- Revision:
- 226:4a4d5bd5fcd7
- Parent:
- 218:b4067cac75c0
- Child:
- 227:2774b56bfab0
File content as of revision 226:4a4d5bd5fcd7:
#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; LPC_GPIOINT->IO0IntClr |= (1<<1); LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn LoopOn } 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; static unsigned int FConunt=0; int LowDZ,HiDZ; /* if(FConunt<4) {*/ //FConunt++; Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddPlus = 0; CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; // } /*else {*/ // FConunt=0; //Gyro.MaxAmp=Gyro.MaxAmp>>2; 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>>4; // расчет амплитуды ВП с учетом разници 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) { static unsigned int counttt=0; switch(CountV31) { case 0: ReVib();///обновление значений вибро Gyro.VibroAMPRegulF=1; Time_vibro=0; Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. break; case 8: 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); //вкл break; case 16: Time_vibro=0; Gyro.VibroFrqRegulF=1; // break; case 31: /* if(counttt>199) { sprintf((Time)," %d %d %d %d \r\n ", SinMns, SinPls, SinMns+SinPls, faza); WriteCon(Time); counttt=0; SinMns=0; SinPls=0; } counttt++; */ 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(); PLCRegul(); if(Gyro.LG_Type==1) { HFORegul(); } } // Регулеровка Амплитуды виброподвеса if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();} }