![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fork
Fork of LG by
vibro.c@202:c03b7b128e11, 2016-11-18 (annotated)
- Committer:
- Kovalev_D
- Date:
- Fri Nov 18 06:07:37 2016 +0000
- Revision:
- 202:c03b7b128e11
- Parent:
- 201:76f4123bf22a
not work
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 0:8ad47e2b6f00 | 1 | #include "Global.h" |
igor_v | 30:17c84ed091b3 | 2 | GyroT Gyro; |
Kovalev_D | 129:406995a91322 | 3 | GyroParam GyroP; |
Kovalev_D | 112:4a96133a1311 | 4 | volatile unsigned int Cheng_AMP_Flag=0; |
Kovalev_D | 190:289514f730ee | 5 | //int reper=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 6 | int Rate2VibFlag,countA=0,tempDP,vibrot=0,fnoize=0,Znak=0,tempy,ttempo; |
Kovalev_D | 191:40028201ddad | 7 | unsigned int OldMaxAmp=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 8 | |
Kovalev_D | 193:a0fe8bfc97e4 | 9 | |
Kovalev_D | 193:a0fe8bfc97e4 | 10 | __irq void EINT3_IRQHandler() |
Kovalev_D | 193:a0fe8bfc97e4 | 11 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 12 | Gyro.DeltaEXT_Event=1; |
Kovalev_D | 193:a0fe8bfc97e4 | 13 | Gyro.B_Delta_EventEXT=1; |
Kovalev_D | 193:a0fe8bfc97e4 | 14 | LPC_GPIOINT->IO0IntClr |= (1<<1); |
Kovalev_D | 193:a0fe8bfc97e4 | 15 | // CMD_Delta_Bins(); |
Kovalev_D | 193:a0fe8bfc97e4 | 16 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 17 | |
Kovalev_D | 202:c03b7b128e11 | 18 | void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
Kovalev_D | 202:c03b7b128e11 | 19 | { |
Kovalev_D | 202:c03b7b128e11 | 20 | static int PeriodCount = 0; |
Kovalev_D | 202:c03b7b128e11 | 21 | unsigned int Nmax=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 22 | |
Kovalev_D | 202:c03b7b128e11 | 23 | //расчет амплитуды относительно центральной точки |
Kovalev_D | 202:c03b7b128e11 | 24 | if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. |
Kovalev_D | 202:c03b7b128e11 | 25 | PeriodCount=0;//сбрасываем таймер |
Kovalev_D | 202:c03b7b128e11 | 26 | |
Kovalev_D | 202:c03b7b128e11 | 27 | if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? |
Kovalev_D | 202:c03b7b128e11 | 28 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { |
Kovalev_D | 202:c03b7b128e11 | 29 | Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды |
Kovalev_D | 202:c03b7b128e11 | 30 | } |
Kovalev_D | 202:c03b7b128e11 | 31 | Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); // |
Kovalev_D | 202:c03b7b128e11 | 32 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 33 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 34 | Cheng_AMP_Flag=1; |
Kovalev_D | 202:c03b7b128e11 | 35 | |
Kovalev_D | 202:c03b7b128e11 | 36 | } |
Kovalev_D | 202:c03b7b128e11 | 37 | |
Kovalev_D | 202:c03b7b128e11 | 38 | else { |
Kovalev_D | 202:c03b7b128e11 | 39 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { |
Kovalev_D | 202:c03b7b128e11 | 40 | Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды |
Kovalev_D | 202:c03b7b128e11 | 41 | } |
Kovalev_D | 202:c03b7b128e11 | 42 | |
Kovalev_D | 202:c03b7b128e11 | 43 | Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 202:c03b7b128e11 | 44 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 45 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 46 | Cheng_AMP_Flag=0; |
Kovalev_D | 202:c03b7b128e11 | 47 | |
Kovalev_D | 202:c03b7b128e11 | 48 | } |
Kovalev_D | 202:c03b7b128e11 | 49 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 202:c03b7b128e11 | 50 | Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 202:c03b7b128e11 | 51 | |
Kovalev_D | 202:c03b7b128e11 | 52 | } else { |
Kovalev_D | 202:c03b7b128e11 | 53 | PeriodCount++;//таймер амплитуды |
Kovalev_D | 202:c03b7b128e11 | 54 | } |
Kovalev_D | 202:c03b7b128e11 | 55 | |
Kovalev_D | 202:c03b7b128e11 | 56 | LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 202:c03b7b128e11 | 57 | } |
Kovalev_D | 202:c03b7b128e11 | 58 | |
Kovalev_D | 202:c03b7b128e11 | 59 | void OLD_CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
igor_v | 0:8ad47e2b6f00 | 60 | { |
Kovalev_D | 202:c03b7b128e11 | 61 | static int PeriodCount = 0; |
Kovalev_D | 202:c03b7b128e11 | 62 | unsigned int Nmax=0,Nmid=0,Nper=0; |
Kovalev_D | 202:c03b7b128e11 | 63 | |
Kovalev_D | 202:c03b7b128e11 | 64 | //расчет амплитуды относительно центральной точки |
Kovalev_D | 202:c03b7b128e11 | 65 | if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления. |
Kovalev_D | 202:c03b7b128e11 | 66 | PeriodCount=0;//сбрасываем таймер |
Kovalev_D | 202:c03b7b128e11 | 67 | |
Kovalev_D | 202:c03b7b128e11 | 68 | if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда? |
Kovalev_D | 202:c03b7b128e11 | 69 | if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { |
Kovalev_D | 202:c03b7b128e11 | 70 | Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды |
Kovalev_D | 202:c03b7b128e11 | 71 | } |
Kovalev_D | 202:c03b7b128e11 | 72 | Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 202:c03b7b128e11 | 73 | Nmid=(unsigned int)(Nmax/2); |
Kovalev_D | 202:c03b7b128e11 | 74 | Nper=(unsigned int)((Nmax*Gyro.AmpPer)/200); |
Kovalev_D | 202:c03b7b128e11 | 75 | if(Nper<1)Nper=1; |
Kovalev_D | 202:c03b7b128e11 | 76 | |
Kovalev_D | 202:c03b7b128e11 | 77 | Gyro.AmpN1=(unsigned int) (Nmid - Nper); |
Kovalev_D | 202:c03b7b128e11 | 78 | Gyro.AmpN2=(unsigned int) (Nmid + Nper); |
Kovalev_D | 202:c03b7b128e11 | 79 | |
Kovalev_D | 202:c03b7b128e11 | 80 | Cheng_AMP_Flag=1; |
Kovalev_D | 202:c03b7b128e11 | 81 | } |
Kovalev_D | 202:c03b7b128e11 | 82 | |
Kovalev_D | 202:c03b7b128e11 | 83 | else { |
Kovalev_D | 202:c03b7b128e11 | 84 | /* if((Gyro.AmpPer+Gyro.AmpPerDel)>90) { |
Kovalev_D | 202:c03b7b128e11 | 85 | Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды |
Kovalev_D | 202:c03b7b128e11 | 86 | }*/ |
Kovalev_D | 202:c03b7b128e11 | 87 | |
Kovalev_D | 202:c03b7b128e11 | 88 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 202:c03b7b128e11 | 89 | Nmid = (unsigned int) (Nmax/2); |
Kovalev_D | 202:c03b7b128e11 | 90 | Nper = (unsigned int)((Nmax*(Gyro.AmpPer + Gyro.AmpPerDel))/200); |
Kovalev_D | 202:c03b7b128e11 | 91 | if(Nper<1)Nper=2; |
Kovalev_D | 202:c03b7b128e11 | 92 | |
Kovalev_D | 202:c03b7b128e11 | 93 | Gyro.AmpN1 =(unsigned int) (Nmid - Nper); |
Kovalev_D | 202:c03b7b128e11 | 94 | Gyro.AmpN2 =(unsigned int) (Nmid + Nper); |
Kovalev_D | 202:c03b7b128e11 | 95 | Cheng_AMP_Flag=0; |
Kovalev_D | 202:c03b7b128e11 | 96 | |
igor_v | 21:bc8c1cec3da6 | 97 | } |
Kovalev_D | 202:c03b7b128e11 | 98 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 202:c03b7b128e11 | 99 | Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 202:c03b7b128e11 | 100 | |
Kovalev_D | 202:c03b7b128e11 | 101 | } else { |
Kovalev_D | 202:c03b7b128e11 | 102 | PeriodCount++;//таймер амплитуды |
Kovalev_D | 202:c03b7b128e11 | 103 | } |
Kovalev_D | 202:c03b7b128e11 | 104 | |
Kovalev_D | 202:c03b7b128e11 | 105 | LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 202:c03b7b128e11 | 106 | } |
Kovalev_D | 85:0466ee8cdfc8 | 107 | |
Kovalev_D | 202:c03b7b128e11 | 108 | void VibroAMPRegul(void) //подстройка амплитуды ВП |
Kovalev_D | 202:c03b7b128e11 | 109 | { |
Kovalev_D | 202:c03b7b128e11 | 110 | Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin |
Kovalev_D | 202:c03b7b128e11 | 111 | CaunAddPlus = 0; |
Kovalev_D | 202:c03b7b128e11 | 112 | Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin |
Kovalev_D | 202:c03b7b128e11 | 113 | CaunAddMin = 0; |
Kovalev_D | 202:c03b7b128e11 | 114 | Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р. |
Kovalev_D | 202:c03b7b128e11 | 115 | if(Gyro.RgConA&0x20) { |
Kovalev_D | 202:c03b7b128e11 | 116 | Gyro.Amp -= (Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed; // расчет амплитуды ВП с учетом разници |
Kovalev_D | 202:c03b7b128e11 | 117 | if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// |
Kovalev_D | 202:c03b7b128e11 | 118 | if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// |
Kovalev_D | 202:c03b7b128e11 | 119 | Gyro.AmpPer = Gyro.Amp>>15; //приведение амплитуды ВП к виду 0%-100% |
Kovalev_D | 202:c03b7b128e11 | 120 | sprintf((Time),"%d %d %d \r\n",Gyro.AmpN1,Gyro.AmpN2,Gyro.AmpPer);//выдаем в терминал для постройки граффика регулировки периметра. |
Kovalev_D | 202:c03b7b128e11 | 121 | WriteCon(Time); |
igor_v | 21:bc8c1cec3da6 | 122 | } |
igor_v | 0:8ad47e2b6f00 | 123 | } |
igor_v | 0:8ad47e2b6f00 | 124 | |
Kovalev_D | 202:c03b7b128e11 | 125 | void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты) |
Kovalev_D | 202:c03b7b128e11 | 126 | { |
Kovalev_D | 202:c03b7b128e11 | 127 | static int TempFaza, CountFaza; |
Kovalev_D | 202:c03b7b128e11 | 128 | TempFaza = -4; |
Kovalev_D | 202:c03b7b128e11 | 129 | for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0 |
Kovalev_D | 202:c03b7b128e11 | 130 | if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;} |
Kovalev_D | 202:c03b7b128e11 | 131 | |
Kovalev_D | 202:c03b7b128e11 | 132 | if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты |
Kovalev_D | 202:c03b7b128e11 | 133 | else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты |
Kovalev_D | 202:c03b7b128e11 | 134 | } |
Kovalev_D | 202:c03b7b128e11 | 135 | |
Kovalev_D | 202:c03b7b128e11 | 136 | ////////////////////////////////////////////////////////////////////////////// |
Kovalev_D | 202:c03b7b128e11 | 137 | /////////////////////////основного 32 тактного цикла////////////////////////// |
Kovalev_D | 202:c03b7b128e11 | 138 | ////////////////////////////////////////////////////////////////////////////// |
Kovalev_D | 202:c03b7b128e11 | 139 | void cheng(void) |
Kovalev_D | 202:c03b7b128e11 | 140 | { |
Kovalev_D | 202:c03b7b128e11 | 141 | switch(CountV31) { |
Kovalev_D | 202:c03b7b128e11 | 142 | case 0: Gyro.VibroAMPRegulF=1; Time_vibro=0; Gyro.VibroNoiseF=1; break; //расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления. |
Kovalev_D | 202:c03b7b128e11 | 143 | case 16: Gyro.Reper_Event=1; Time_vibro=0; Gyro.VibroFrqRegulF=1; break; |
Kovalev_D | 202:c03b7b128e11 | 144 | } |
Kovalev_D | 202:c03b7b128e11 | 145 | } |
Kovalev_D | 202:c03b7b128e11 | 146 | void AllRegul (void) |
Kovalev_D | 202:c03b7b128e11 | 147 | { ///////////////////////////контуры регулировки///////////////////////////// |
Kovalev_D | 202:c03b7b128e11 | 148 | |
Kovalev_D | 202:c03b7b128e11 | 149 | if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро |
Kovalev_D | 202:c03b7b128e11 | 150 | if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD |
Kovalev_D | 202:c03b7b128e11 | 151 | |
Kovalev_D | 202:c03b7b128e11 | 152 | if (Gyro.ADS_NewData == 1) |
Kovalev_D | 202:c03b7b128e11 | 153 | { Gyro.ADS_NewData = 0; |
Kovalev_D | 202:c03b7b128e11 | 154 | |
Kovalev_D | 202:c03b7b128e11 | 155 | }/// был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра. |
Kovalev_D | 202:c03b7b128e11 | 156 | |
Kovalev_D | 202:c03b7b128e11 | 157 | |
Kovalev_D | 202:c03b7b128e11 | 158 | |
Kovalev_D | 202:c03b7b128e11 | 159 | if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса |
Kovalev_D | 202:c03b7b128e11 | 160 | if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса |
Kovalev_D | 202:c03b7b128e11 | 161 | if (Gyro.VibroNoiseF == 1) |
Kovalev_D | 202:c03b7b128e11 | 162 | { |
Kovalev_D | 202:c03b7b128e11 | 163 | switch(Gyro.flag) { |
Kovalev_D | 202:c03b7b128e11 | 164 | case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break; |
Kovalev_D | 202:c03b7b128e11 | 165 | case 2: Gyro.AmpMin =3;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;Calc2AmpN(); break; |
Kovalev_D | 202:c03b7b128e11 | 166 | case 3: Gyro.AmpMin =1;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;CalcAmpD(); break; |
Kovalev_D | 202:c03b7b128e11 | 167 | } |
Kovalev_D | 202:c03b7b128e11 | 168 | } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды |
Kovalev_D | 202:c03b7b128e11 | 169 | //if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut();} // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно |
Kovalev_D | 202:c03b7b128e11 | 170 | } |
Kovalev_D | 202:c03b7b128e11 | 171 | |
Kovalev_D | 191:40028201ddad | 172 | |
Kovalev_D | 202:c03b7b128e11 | 173 | |
Kovalev_D | 202:c03b7b128e11 | 174 | |
Kovalev_D | 202:c03b7b128e11 | 175 | |
Kovalev_D | 202:c03b7b128e11 | 176 | |
Kovalev_D | 202:c03b7b128e11 | 177 | |
Kovalev_D | 202:c03b7b128e11 | 178 | |
Kovalev_D | 202:c03b7b128e11 | 179 | |
Kovalev_D | 202:c03b7b128e11 | 180 | |
Kovalev_D | 202:c03b7b128e11 | 181 | |
Kovalev_D | 202:c03b7b128e11 | 182 | |
Kovalev_D | 202:c03b7b128e11 | 183 | |
Kovalev_D | 202:c03b7b128e11 | 184 | |
Kovalev_D | 202:c03b7b128e11 | 185 | |
Kovalev_D | 202:c03b7b128e11 | 186 | |
Kovalev_D | 202:c03b7b128e11 | 187 | |
Kovalev_D | 202:c03b7b128e11 | 188 | |
Kovalev_D | 202:c03b7b128e11 | 189 | |
Kovalev_D | 202:c03b7b128e11 | 190 | |
Kovalev_D | 202:c03b7b128e11 | 191 | |
Kovalev_D | 202:c03b7b128e11 | 192 | |
Kovalev_D | 202:c03b7b128e11 | 193 | |
Kovalev_D | 202:c03b7b128e11 | 194 | |
Kovalev_D | 202:c03b7b128e11 | 195 | |
Kovalev_D | 202:c03b7b128e11 | 196 | |
Kovalev_D | 202:c03b7b128e11 | 197 | |
Kovalev_D | 202:c03b7b128e11 | 198 | |
Kovalev_D | 202:c03b7b128e11 | 199 | |
Kovalev_D | 202:c03b7b128e11 | 200 | |
Kovalev_D | 202:c03b7b128e11 | 201 | |
Kovalev_D | 202:c03b7b128e11 | 202 | |
Kovalev_D | 202:c03b7b128e11 | 203 | |
Kovalev_D | 202:c03b7b128e11 | 204 | void CalcAmpI(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
Kovalev_D | 193:a0fe8bfc97e4 | 205 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 206 | Gyro.AmpSC=0; |
Kovalev_D | 202:c03b7b128e11 | 207 | static int PeriodCount = 0 ; |
Kovalev_D | 193:a0fe8bfc97e4 | 208 | unsigned int Nmax=0; |
Kovalev_D | 202:c03b7b128e11 | 209 | Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ; |
Kovalev_D | 202:c03b7b128e11 | 210 | if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); |
Kovalev_D | 202:c03b7b128e11 | 211 | OldMaxAmp=Gyro.MaxAmp; |
Kovalev_D | 193:a0fe8bfc97e4 | 212 | |
Kovalev_D | 202:c03b7b128e11 | 213 | countA++; |
Kovalev_D | 202:c03b7b128e11 | 214 | if( Gyro.AmpSC<60) |
Kovalev_D | 202:c03b7b128e11 | 215 | { |
Kovalev_D | 202:c03b7b128e11 | 216 | // countA=0; |
Kovalev_D | 191:40028201ddad | 217 | |
Kovalev_D | 191:40028201ddad | 218 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 191:40028201ddad | 219 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды |
Kovalev_D | 191:40028201ddad | 220 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 191:40028201ddad | 221 | Cheng_AMP_Flag=1; |
Kovalev_D | 193:a0fe8bfc97e4 | 222 | |
Kovalev_D | 202:c03b7b128e11 | 223 | // tempDP=Gyro.AmpPerDel; |
Kovalev_D | 202:c03b7b128e11 | 224 | // srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 202:c03b7b128e11 | 225 | if(Gyro.flag==1) Gyro.AmpPerDel = 1; |
Kovalev_D | 202:c03b7b128e11 | 226 | |
Kovalev_D | 202:c03b7b128e11 | 227 | switch(Znak) { |
Kovalev_D | 202:c03b7b128e11 | 228 | case 0: |
Kovalev_D | 202:c03b7b128e11 | 229 | Gyro.AmpPerDel--; |
Kovalev_D | 202:c03b7b128e11 | 230 | if (Gyro.AmpPerDel<1)Znak=1; |
Kovalev_D | 202:c03b7b128e11 | 231 | break; |
Kovalev_D | 202:c03b7b128e11 | 232 | |
Kovalev_D | 202:c03b7b128e11 | 233 | case 1: |
Kovalev_D | 202:c03b7b128e11 | 234 | Gyro.AmpPerDel++; |
Kovalev_D | 202:c03b7b128e11 | 235 | if (Gyro.AmpPerDel>13)Znak=0; |
Kovalev_D | 202:c03b7b128e11 | 236 | break; |
Kovalev_D | 202:c03b7b128e11 | 237 | } |
Kovalev_D | 202:c03b7b128e11 | 238 | |
Kovalev_D | 193:a0fe8bfc97e4 | 239 | } //8046 |
Kovalev_D | 193:a0fe8bfc97e4 | 240 | LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 193:a0fe8bfc97e4 | 241 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 242 | |
Kovalev_D | 193:a0fe8bfc97e4 | 243 | |
Kovalev_D | 193:a0fe8bfc97e4 | 244 | void CalcAmpD(void) |
Kovalev_D | 193:a0fe8bfc97e4 | 245 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 246 | unsigned int Nmax=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 247 | countA++; |
Kovalev_D | 193:a0fe8bfc97e4 | 248 | if( countA>1) |
Kovalev_D | 193:a0fe8bfc97e4 | 249 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 250 | countA=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 251 | |
Kovalev_D | 193:a0fe8bfc97e4 | 252 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 193:a0fe8bfc97e4 | 253 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды |
Kovalev_D | 193:a0fe8bfc97e4 | 254 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 193:a0fe8bfc97e4 | 255 | Cheng_AMP_Flag=1; |
Kovalev_D | 193:a0fe8bfc97e4 | 256 | |
Kovalev_D | 193:a0fe8bfc97e4 | 257 | // if(Gyro.flag==1) Gyro.AmpPerDel = 1; |
Kovalev_D | 193:a0fe8bfc97e4 | 258 | |
Kovalev_D | 193:a0fe8bfc97e4 | 259 | |
Kovalev_D | 193:a0fe8bfc97e4 | 260 | switch(Znak) { |
Kovalev_D | 193:a0fe8bfc97e4 | 261 | case 0: |
Kovalev_D | 193:a0fe8bfc97e4 | 262 | Gyro.AmpPerDel++; |
Kovalev_D | 193:a0fe8bfc97e4 | 263 | if (Gyro.AmpPerDel>10){Znak=1; fnoize++;} |
Kovalev_D | 193:a0fe8bfc97e4 | 264 | break; |
Kovalev_D | 193:a0fe8bfc97e4 | 265 | case 1: |
Kovalev_D | 193:a0fe8bfc97e4 | 266 | Gyro.AmpPerDel--; |
Kovalev_D | 193:a0fe8bfc97e4 | 267 | if (Gyro.AmpPerDel<1)Znak=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 268 | if (fnoize>6)Znak=2; |
Kovalev_D | 193:a0fe8bfc97e4 | 269 | break; |
Kovalev_D | 196:f76dbc081e63 | 270 | |
Kovalev_D | 193:a0fe8bfc97e4 | 271 | case 2: |
Kovalev_D | 193:a0fe8bfc97e4 | 272 | Gyro.AmpPerDel++; |
Kovalev_D | 193:a0fe8bfc97e4 | 273 | if (Gyro.AmpPerDel>7){Znak=3; fnoize++;} |
Kovalev_D | 193:a0fe8bfc97e4 | 274 | break; |
Kovalev_D | 193:a0fe8bfc97e4 | 275 | case 3: |
Kovalev_D | 193:a0fe8bfc97e4 | 276 | Gyro.AmpPerDel--; |
Kovalev_D | 193:a0fe8bfc97e4 | 277 | if (Gyro.AmpPerDel<1)Znak=2; |
Kovalev_D | 193:a0fe8bfc97e4 | 278 | if (fnoize>12){Znak=4;/*fnoize=0;*/} |
Kovalev_D | 193:a0fe8bfc97e4 | 279 | break; |
Kovalev_D | 196:f76dbc081e63 | 280 | |
Kovalev_D | 193:a0fe8bfc97e4 | 281 | case 4: |
Kovalev_D | 193:a0fe8bfc97e4 | 282 | Gyro.AmpPerDel++; |
Kovalev_D | 193:a0fe8bfc97e4 | 283 | if (Gyro.AmpPerDel>15){Znak=5; fnoize++;} |
Kovalev_D | 193:a0fe8bfc97e4 | 284 | break; |
Kovalev_D | 193:a0fe8bfc97e4 | 285 | case 5: |
Kovalev_D | 193:a0fe8bfc97e4 | 286 | Gyro.AmpPerDel--; |
Kovalev_D | 193:a0fe8bfc97e4 | 287 | if (Gyro.AmpPerDel<1)Znak=4; |
Kovalev_D | 193:a0fe8bfc97e4 | 288 | if (fnoize>18){Znak=6;/*fnoize=0;*/} |
Kovalev_D | 193:a0fe8bfc97e4 | 289 | break; |
Kovalev_D | 196:f76dbc081e63 | 290 | |
Kovalev_D | 193:a0fe8bfc97e4 | 291 | case 6: |
Kovalev_D | 193:a0fe8bfc97e4 | 292 | Gyro.AmpPerDel++; |
Kovalev_D | 193:a0fe8bfc97e4 | 293 | if (Gyro.AmpPerDel>6){Znak=7;fnoize++;} |
Kovalev_D | 193:a0fe8bfc97e4 | 294 | break; |
Kovalev_D | 193:a0fe8bfc97e4 | 295 | case 7: |
Kovalev_D | 193:a0fe8bfc97e4 | 296 | Gyro.AmpPerDel--; |
Kovalev_D | 193:a0fe8bfc97e4 | 297 | if (Gyro.AmpPerDel<1)Znak=6; |
Kovalev_D | 193:a0fe8bfc97e4 | 298 | if (fnoize>24){Znak=0;fnoize=0;} |
Kovalev_D | 193:a0fe8bfc97e4 | 299 | break; |
Kovalev_D | 193:a0fe8bfc97e4 | 300 | } |
Kovalev_D | 191:40028201ddad | 301 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 302 | LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 193:a0fe8bfc97e4 | 303 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 304 | |
Kovalev_D | 193:a0fe8bfc97e4 | 305 | |
Kovalev_D | 193:a0fe8bfc97e4 | 306 | |
Kovalev_D | 202:c03b7b128e11 | 307 | void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
Kovalev_D | 193:a0fe8bfc97e4 | 308 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 309 | Gyro.AmpSC=0; |
Kovalev_D | 202:c03b7b128e11 | 310 | static int PeriodCount = 0; |
Kovalev_D | 193:a0fe8bfc97e4 | 311 | unsigned int Nmax=0; |
Kovalev_D | 202:c03b7b128e11 | 312 | Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp; |
Kovalev_D | 202:c03b7b128e11 | 313 | if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); |
Kovalev_D | 202:c03b7b128e11 | 314 | OldMaxAmp=Gyro.MaxAmp; |
Kovalev_D | 193:a0fe8bfc97e4 | 315 | |
Kovalev_D | 202:c03b7b128e11 | 316 | if(Gyro.AmpSC <5)countA++; |
Kovalev_D | 202:c03b7b128e11 | 317 | if(countA >3) |
Kovalev_D | 202:c03b7b128e11 | 318 | { |
Kovalev_D | 202:c03b7b128e11 | 319 | countA=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 320 | |
Kovalev_D | 193:a0fe8bfc97e4 | 321 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 193:a0fe8bfc97e4 | 322 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды |
Kovalev_D | 193:a0fe8bfc97e4 | 323 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 193:a0fe8bfc97e4 | 324 | Cheng_AMP_Flag=1; |
Kovalev_D | 193:a0fe8bfc97e4 | 325 | |
Kovalev_D | 202:c03b7b128e11 | 326 | tempDP=Gyro.AmpPerDel; |
Kovalev_D | 202:c03b7b128e11 | 327 | srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин. |
Kovalev_D | 202:c03b7b128e11 | 328 | if(Gyro.flag==1) Gyro.AmpPerDel = 1; |
Kovalev_D | 202:c03b7b128e11 | 329 | else Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 202:c03b7b128e11 | 330 | } //8046 |
Kovalev_D | 193:a0fe8bfc97e4 | 331 | |
Kovalev_D | 193:a0fe8bfc97e4 | 332 | LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
Kovalev_D | 193:a0fe8bfc97e4 | 333 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 334 | |
Kovalev_D | 193:a0fe8bfc97e4 | 335 | |
Kovalev_D | 193:a0fe8bfc97e4 | 336 | |
Kovalev_D | 202:c03b7b128e11 | 337 | void Calc2AmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления. |
Kovalev_D | 193:a0fe8bfc97e4 | 338 | { |
Kovalev_D | 202:c03b7b128e11 | 339 | Gyro.AmpSC=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 340 | static int PeriodCount = 0; |
Kovalev_D | 193:a0fe8bfc97e4 | 341 | unsigned int Nmax=0; |
Kovalev_D | 202:c03b7b128e11 | 342 | Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ; |
Kovalev_D | 202:c03b7b128e11 | 343 | if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1); |
Kovalev_D | 202:c03b7b128e11 | 344 | OldMaxAmp=Gyro.MaxAmp; |
Kovalev_D | 202:c03b7b128e11 | 345 | |
Kovalev_D | 202:c03b7b128e11 | 346 | if(Gyro.AmpSC <55)countA++; |
Kovalev_D | 202:c03b7b128e11 | 347 | if(countA >2) |
Kovalev_D | 202:c03b7b128e11 | 348 | { |
Kovalev_D | 202:c03b7b128e11 | 349 | countA=0; |
Kovalev_D | 202:c03b7b128e11 | 350 | srand(Global_Time); |
Kovalev_D | 202:c03b7b128e11 | 351 | if(Cheng_AMP_Flag) |
Kovalev_D | 202:c03b7b128e11 | 352 | { |
Kovalev_D | 202:c03b7b128e11 | 353 | Cheng_AMP_Flag=0; |
Kovalev_D | 202:c03b7b128e11 | 354 | Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp |
Kovalev_D | 202:c03b7b128e11 | 355 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 202:c03b7b128e11 | 356 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 357 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 193:a0fe8bfc97e4 | 358 | } |
Kovalev_D | 202:c03b7b128e11 | 359 | else |
Kovalev_D | 202:c03b7b128e11 | 360 | { |
Kovalev_D | 202:c03b7b128e11 | 361 | Cheng_AMP_Flag=1; |
Kovalev_D | 202:c03b7b128e11 | 362 | Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1); |
Kovalev_D | 202:c03b7b128e11 | 363 | Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 364 | Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды |
Kovalev_D | 202:c03b7b128e11 | 365 | } |
Kovalev_D | 202:c03b7b128e11 | 366 | } //8046 |
Kovalev_D | 202:c03b7b128e11 | 367 | |
Kovalev_D | 202:c03b7b128e11 | 368 | LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро |
igor_v | 0:8ad47e2b6f00 | 369 | } |
igor_v | 0:8ad47e2b6f00 | 370 |