Dmitry Kovalev
/
LGstaandart
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
LG2
by 
Dmitry Kovalev
Embed:
(wiki syntax)
Show/hide line numbers
vibro.c
00001 #include "Global.h" 00002 GyroT Gyro; 00003 GyroParam GyroP; 00004 volatile unsigned int Cheng_AMP_Flag=0; 00005 //int reper=0; 00006 int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo; 00007 unsigned int OldMaxAmp=0,countFras=0; 00008 int z=25; 00009 int i=16,tempi=0,klk=0; 00010 00011 00012 __irq void EINT3_IRQHandler() 00013 { 00014 00015 Gyro.EXT_Latch=1; 00016 LPC_GPIOINT->IO0IntClr |= (1<<1); 00017 00018 } 00019 00020 00021 void VibroOut(void) // выставка ног вибро 00022 { 00023 if(CountV31>=16) 00024 {//первая нога вибро 00025 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1 00026 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2)) 00027 { 00028 SetV1//установить в регистре PinReg бит "вибро 1" в "0" 00029 } 00030 else 00031 { 00032 ClrV1 //установить в регистре PinReg бит "вибро 1" в "1" 00033 } 00034 00035 } 00036 else {//вторая нога вибро 00037 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2)) 00038 { 00039 SetV2 //установить в регистре PinReg бит "вибро 2" в "0" 00040 } 00041 else 00042 { 00043 ClrV2//установить в регистре PinReg бит "вибро 2" в "1" 00044 } 00045 } 00046 } 00047 /* 00048 00049 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. 00050 { 00051 static int PeriodCount = 0,Period=0; 00052 unsigned int Nmax=0, lowper=1; 00053 Gyro.FrqHZ=Gyro.Frq>>16; 00054 if(PeriodCount>= Gyro.AmpT) 00055 { //если количество заходов в прерывание больше либо равно частоте ошумления. 00056 PeriodCount=0;//сбрасываем таймер 00057 sprintf((Time),"%d %d %d\r\n", Gyro.AmpN1, Gyro.AmpN2-Gyro.AmpN1, Gyro.AmpPer); 00058 WriteCon(Time); 00059 //Gyro.AmpPerDel=(Gyro.AmpPer*100)/Gyro.AmpPerDel; 00060 if(Cheng_AMP_Flag==0) 00061 { 00062 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды 00063 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);//256 // 00064 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды 63 00065 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 65 00066 Gyro.L_vibro=Gyro.AmpPer*208; 00067 sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); 00068 WriteCon(Time); 00069 Cheng_AMP_Flag=1; 00070 } 00071 else 00072 { 00073 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды 00074 if((Gyro.RgConA&0x20)) 00075 { 00076 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);//256 00077 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/Gyro.FrqHZ); //левая граница амплитуды 61 00078 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды 67 00079 Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*208; 00080 sprintf((Time),"%d %d %d \r\n",Gyro.L_vibro,Gyro.AmpPer,Cheng_AMP_Flag); 00081 WriteCon(Time); 00082 } 00083 Cheng_AMP_Flag=0; 00084 } 00085 if(Gyro.AmpN2<(Gyro.AmpN1+2))Gyro.AmpN2=Gyro.AmpN1+2; 00086 00087 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. 00088 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp 00089 } else { 00090 PeriodCount++;//таймер амплитуды 00091 } 00092 }*/ 00093 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. 00094 { 00095 static int PeriodCount = 0,Period=0; 00096 unsigned int Nmax=0, lowper=1; 00097 Gyro.FrqHZ=Gyro.Frq>>16; 00098 if(PeriodCount>= Gyro.AmpT) 00099 { 00100 PeriodCount=0;//сбрасываем таймер 00101 if(Cheng_AMP_Flag==0) 00102 { 00103 //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды 00104 T_vib_1 = Gyro.AmpPer * T_vibP; 00105 T_vib_2 = T_vibP * (10000-Gyro.AmpPer); 00106 Gyro.L_vibro=Gyro.AmpPer*3; 00107 Cheng_AMP_Flag=1; 00108 } 00109 else 00110 { 00111 //if((Gyro.AmpPer+Gyro.AmpPerDel*100)>9000) Gyro.AmpPer=9000-Gyro.AmpPerDel*100; //проверка верхней граници амплитуды 00112 if((Gyro.RgConA&0x20)) 00113 { 00114 T_vib_1 = T_vibP * (Gyro.AmpPer +(Gyro.AmpPerDel*100)); 00115 T_vib_2 = T_vibP * (10000 -(Gyro.AmpPer+(Gyro.AmpPerDel*100))); 00116 Gyro.L_vibro=(Gyro.AmpPer+Gyro.AmpPerDel)*3; 00117 } 00118 Cheng_AMP_Flag=0; 00119 } 00120 00121 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. 00122 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp 00123 } 00124 else {PeriodCount++;} 00125 00126 } 00127 00128 void VibroAMPRegul(void) //подстройка амплитуды ВП 00129 { 00130 00131 int temp=0; 00132 static unsigned int FConunt=0; 00133 int LowDZ,HiDZ; 00134 /* if(FConunt<4) 00135 {*/ 00136 //FConunt++; 00137 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin 00138 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin 00139 CaunAddPlus = 0; 00140 CaunAddMin = 0; 00141 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; 00142 // } 00143 /*else 00144 {*/ 00145 // FConunt=0; 00146 //Gyro.MaxAmp=Gyro.MaxAmp>>2; 00147 if(countFras<512) 00148 { 00149 countFras++; 00150 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; 00151 } 00152 else 00153 { 00154 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; 00155 Gyro.F_ras=Gyro.F_rasAdd>>9; 00156 Gyro.F_rasAdd=0; 00157 countFras=0; 00158 } 00159 if(Gyro.RgConA&0x20) 00160 { 00161 //расчет максимальной амплитуды из востановленного синуса р-р. 00162 temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed)); 00163 temp=temp>>6; 00164 LowDZ = ((Gyro.AmpSpeed<<3)*(-1)); 00165 HiDZ = (Gyro.AmpSpeed<<3); 00166 Gyro.Amp -= temp>>4; // расчет амплитуды ВП с учетом разници(Gyro.AmpPer<<17)/100; 00167 if((Gyro.AmpPer) > (Gyro.AmpPerMax)) {Gyro.Amp = ((Gyro.AmpPerMax<<17)/100);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// 00168 if((Gyro.AmpPer) < (Gyro.AmpPerMin)) {Gyro.Amp = ((Gyro.AmpPerMin<<17)/100);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// 00169 // Gyro.AmpPer = (Gyro.Amp)>>16; //приведение амплитуды ВП к виду 0%-100% 00170 } 00171 LPC_MCPWM->MAT1 = T_vib_1; 00172 LPC_MCPWM->MAT2 = T_vib_2; 00173 // } 00174 } 00175 00176 00177 00178 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) 00179 { 00180 static int TempFaza, CountFaza; 00181 TempFaza = -4; 00182 Gyro.FrqPhaseEror=0; 00183 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0 00184 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f];} 00185 if(Gyro.RgConA&0x40) 00186 { //12 00187 Gyro.Frq += TempFaza*Gyro.FrqChengSpeed; 00188 } 00189 // Gyro.FrqPhaseEror = TempFaza<<10; 00190 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты 00191 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты*/ 00192 LPC_TIM1->MR0 =(unsigned int)(103200000/(Gyro.Frq>>11));//запись в таймер нового значение частоты вибро 00193 // LPC_TIM1->MR0 =(unsigned int) F_vib; 00194 } 00195 00196 ////////////////////////////////////////////////////////////////////////////// 00197 /////////////////////////основного 32 тактного цикла////////////////////////// 00198 ////////////////////////////////////////////////////////////////////////////// 00199 void cheng(void) 00200 { static unsigned int counttt=0; 00201 switch(CountV31) { 00202 case 0: 00203 00204 ReVib();///обновление значений вибро 00205 Gyro.VibroAMPRegulF=1; 00206 Time_vibro=0; 00207 Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. 00208 break; 00209 case 8: 00210 LPC_MCPWM->CON_CLR |= (1<<8); 00211 LPC_MCPWM->CON_CLR |= (1<<16); 00212 00213 LPC_MCPWM->TC1 =1; 00214 LPC_MCPWM->TC2 =1; 00215 00216 LPC_MCPWM->CON_SET |= (1<<8); 00217 LPC_MCPWM->CON_SET |= (1<<16); //вкл 00218 break; 00219 00220 case 16: 00221 Time_vibro=0; 00222 Gyro.VibroFrqRegulF=1; // 00223 break; 00224 00225 case 31: 00226 /* if(counttt>199) 00227 { 00228 00229 sprintf((Time)," %d %d %d %d \r\n ", SinMns, SinPls, SinMns+SinPls, faza); 00230 WriteCon(Time); 00231 counttt=0; 00232 SinMns=0; 00233 SinPls=0; 00234 } 00235 counttt++; 00236 */ 00237 break; 00238 } 00239 } 00240 void AllRegul (void) 00241 { ///////////////////////////контуры регулировки///////////////////////////// 00242 00243 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро 00244 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD 00245 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса 00246 if (Gyro.VibroAMPRegulF == 1) 00247 { 00248 Gyro.VibroAMPRegulF = 0; 00249 VibroAMPRegul(); 00250 PLCRegul(); 00251 if(Gyro.LG_Type==1) 00252 { 00253 HFORegul(); 00254 } 00255 } // Регулеровка Амплитуды виброподвеса 00256 if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();} 00257 }
Generated on Thu Jul 14 2022 02:34:42 by
1.7.2