Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

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?

UserRevisionLine numberNew 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