Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@209:224e7331a061, 2017-04-13 (annotated)

Committer:
Kovalev_D
Date:
Thu Apr 13 14:14:45 2017 +0000
Revision:
209:224e7331a061
Parent:
208:19150d2b528f
Child:
210:b02fa166315d
v2

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 209:224e7331a061 7 unsigned int OldMaxAmp=0,countFras=0;
Kovalev_D 207:d1ce992f5d17 8 int z=25;
Kovalev_D 208:19150d2b528f 9 int i=16,tempi=0,klk=0;
Kovalev_D 193:a0fe8bfc97e4 10 __irq void EINT3_IRQHandler()
Kovalev_D 208:19150d2b528f 11 {
Kovalev_D 208:19150d2b528f 12 Gyro.EXT_Latch=1;
Kovalev_D 205:775d54fdf646 13 // Gyro.DeltaEXT_Event=1;
Kovalev_D 205:775d54fdf646 14 // Gyro.B_Delta_EventEXT=1;
Kovalev_D 193:a0fe8bfc97e4 15 LPC_GPIOINT->IO0IntClr |= (1<<1);
Kovalev_D 193:a0fe8bfc97e4 16 // CMD_Delta_Bins();
Kovalev_D 193:a0fe8bfc97e4 17 }
Kovalev_D 193:a0fe8bfc97e4 18
Kovalev_D 193:a0fe8bfc97e4 19
igor_v 114:5cc38a53d8a7 20 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 21 {
Kovalev_D 190:289514f730ee 22 if(CountV31>=16)
Kovalev_D 190:289514f730ee 23 {//первая нога вибро
Kovalev_D 89:a0d344db227e 24 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
Kovalev_D 190:289514f730ee 25 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 26 {
Kovalev_D 203:3a6615de9581 27 SetV1//установить в регистре PinReg бит "вибро 1" в "0"
Kovalev_D 190:289514f730ee 28 }
Kovalev_D 190:289514f730ee 29 else
Kovalev_D 190:289514f730ee 30 {
Kovalev_D 207:d1ce992f5d17 31 ClrV1 //установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 32 }
Kovalev_D 85:0466ee8cdfc8 33
Kovalev_D 190:289514f730ee 34 }
Kovalev_D 190:289514f730ee 35 else {//вторая нога вибро
Kovalev_D 190:289514f730ee 36 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 37 {
Kovalev_D 207:d1ce992f5d17 38 SetV2 //установить в регистре PinReg бит "вибро 2" в "0"
Kovalev_D 190:289514f730ee 39 }
Kovalev_D 190:289514f730ee 40 else
Kovalev_D 190:289514f730ee 41 {
Kovalev_D 203:3a6615de9581 42 ClrV2//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 43 }
igor_v 21:bc8c1cec3da6 44 }
igor_v 0:8ad47e2b6f00 45 }
igor_v 0:8ad47e2b6f00 46
Kovalev_D 193:a0fe8bfc97e4 47
Kovalev_D 193:a0fe8bfc97e4 48 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 207:d1ce992f5d17 49 {
Kovalev_D 207:d1ce992f5d17 50 static int PeriodCount = 0,Period=0;
Kovalev_D 204:e7e9762bf609 51 unsigned int Nmax=0, lowper=0;
Kovalev_D 209:224e7331a061 52 Gyro.FrqHZ=Gyro.Frq>>16;
Kovalev_D 209:224e7331a061 53 if(PeriodCount>= Gyro.AmpT)
Kovalev_D 209:224e7331a061 54 { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 209:224e7331a061 55 PeriodCount=0;//сбрасываем таймер
Kovalev_D 209:224e7331a061 56 if(Cheng_AMP_Flag==0)
Kovalev_D 209:224e7331a061 57 {
Kovalev_D 209:224e7331a061 58 if((Gyro.AmpPer+Gyro.AmpPerDel)>90)
Kovalev_D 209:224e7331a061 59 {
Kovalev_D 193:a0fe8bfc97e4 60 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 61 }
Kovalev_D 209:224e7331a061 62 Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1); //
Kovalev_D 209:224e7331a061 63 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды
Kovalev_D 209:224e7331a061 64 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 209:224e7331a061 65 if(Gyro.AmpPer<5)
Kovalev_D 209:224e7331a061 66 {
Kovalev_D 203:3a6615de9581 67 lowper = Gyro.AmpN2-Gyro.AmpN1;
Kovalev_D 203:3a6615de9581 68 lowper=lowper/2;
Kovalev_D 203:3a6615de9581 69 Gyro.AmpN2= Gyro.AmpN2+lowper;
Kovalev_D 203:3a6615de9581 70 }
Kovalev_D 209:224e7331a061 71 Cheng_AMP_Flag=1;
Kovalev_D 209:224e7331a061 72 Gyro.L_vibro=(((16383 *(Gyro.AmpN2-Gyro.AmpN1))/(Nmax/2)));
Kovalev_D 209:224e7331a061 73 // Gyro.L_vibro= Gyro.L_vibro/*2*/;
Kovalev_D 209:224e7331a061 74 }
Kovalev_D 193:a0fe8bfc97e4 75
Kovalev_D 208:19150d2b528f 76 else
Kovalev_D 208:19150d2b528f 77 {
Kovalev_D 193:a0fe8bfc97e4 78 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 79 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 80 }
Kovalev_D 209:224e7331a061 81 if(/*(Gyro.RgConA&0x40)&&*/(Gyro.RgConA&0x20))
Kovalev_D 208:19150d2b528f 82 {
Kovalev_D 207:d1ce992f5d17 83 Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 84 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 85 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 208:19150d2b528f 86 if(Gyro.AmpPer<5)
Kovalev_D 208:19150d2b528f 87 {
Kovalev_D 203:3a6615de9581 88 lowper = Gyro.AmpN2-Gyro.AmpN1;
Kovalev_D 203:3a6615de9581 89 lowper=lowper/2;
Kovalev_D 203:3a6615de9581 90 Gyro.AmpN2= Gyro.AmpN2+lowper+1;
Kovalev_D 208:19150d2b528f 91 }
Kovalev_D 203:3a6615de9581 92 }
Kovalev_D 209:224e7331a061 93
Kovalev_D 203:3a6615de9581 94 // Gyro.AmpN2=Gyro.AmpN1+2;
Kovalev_D 193:a0fe8bfc97e4 95 Cheng_AMP_Flag=0;
Kovalev_D 208:19150d2b528f 96 //Gyro.L_vibro=(103000/Nmax);
Kovalev_D 209:224e7331a061 97 Gyro.L_vibro=(((16383 *(Gyro.AmpN2-Gyro.AmpN1))/(Nmax/2)));
Kovalev_D 209:224e7331a061 98 // Gyro.L_vibro=(((Gyro.AmpPer*7680000)/25)*8)/(Gyro.Frq>>12);
Kovalev_D 209:224e7331a061 99 // Gyro.L_vibro= Gyro.L_vibro/*2*/;
Kovalev_D 207:d1ce992f5d17 100 }
Kovalev_D 209:224e7331a061 101 /* sprintf((Time)," %d %d\r\n",(16383 *((Gyro.AmpN2-Gyro.AmpN1)/Nmax)), Gyro.L_vibro);
Kovalev_D 209:224e7331a061 102 WriteCon(Time);*/
Kovalev_D 207:d1ce992f5d17 103
Kovalev_D 207:d1ce992f5d17 104 // Period=Gyro.CuruAngle*101;
Kovalev_D 207:d1ce992f5d17 105
Kovalev_D 207:d1ce992f5d17 106 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 207:d1ce992f5d17 107 // srand(Mrand());
Kovalev_D 207:d1ce992f5d17 108 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 207:d1ce992f5d17 109
Kovalev_D 193:a0fe8bfc97e4 110 } else {
Kovalev_D 193:a0fe8bfc97e4 111 PeriodCount++;//таймер амплитуды
Kovalev_D 193:a0fe8bfc97e4 112 }
Kovalev_D 208:19150d2b528f 113
Kovalev_D 192:d32c8cf7bcd9 114 }
Kovalev_D 207:d1ce992f5d17 115
Kovalev_D 112:4a96133a1311 116 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 208:19150d2b528f 117 {
Kovalev_D 209:224e7331a061 118
Kovalev_D 208:19150d2b528f 119 int temp=0;
Kovalev_D 209:224e7331a061 120
Kovalev_D 189:8a16378724c4 121 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 209:224e7331a061 122
Kovalev_D 112:4a96133a1311 123 CaunAddPlus = 0;
Kovalev_D 209:224e7331a061 124 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 209:224e7331a061 125
Kovalev_D 112:4a96133a1311 126 CaunAddMin = 0;
Kovalev_D 208:19150d2b528f 127 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;
Kovalev_D 209:224e7331a061 128
Kovalev_D 209:224e7331a061 129
Kovalev_D 209:224e7331a061 130 if(countFras<512)
Kovalev_D 209:224e7331a061 131 {
Kovalev_D 209:224e7331a061 132 countFras++;
Kovalev_D 209:224e7331a061 133 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/22.5;
Kovalev_D 209:224e7331a061 134 }
Kovalev_D 209:224e7331a061 135 else
Kovalev_D 209:224e7331a061 136 {
Kovalev_D 209:224e7331a061 137
Kovalev_D 209:224e7331a061 138 Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/22;
Kovalev_D 209:224e7331a061 139 Gyro.F_ras=Gyro.F_rasAdd>>9;
Kovalev_D 209:224e7331a061 140 Gyro.F_rasAdd=0;
Kovalev_D 209:224e7331a061 141 countFras=0;
Kovalev_D 209:224e7331a061 142 }
Kovalev_D 209:224e7331a061 143
Kovalev_D 209:224e7331a061 144
Kovalev_D 208:19150d2b528f 145 if(Gyro.RgConA&0x20)
Kovalev_D 208:19150d2b528f 146 {
Kovalev_D 209:224e7331a061 147 //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 208:19150d2b528f 148 temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed));
Kovalev_D 208:19150d2b528f 149 temp=temp>>5;
Kovalev_D 208:19150d2b528f 150 Gyro.Amp -= temp; // расчет амплитуды ВП с учетом разници
Kovalev_D 209:224e7331a061 151 if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 209:224e7331a061 152 if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 208:19150d2b528f 153 Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 208:19150d2b528f 154 }
Kovalev_D 208:19150d2b528f 155
Kovalev_D 209:224e7331a061 156
Kovalev_D 209:224e7331a061 157
Kovalev_D 208:19150d2b528f 158 }
Kovalev_D 112:4a96133a1311 159
Kovalev_D 191:40028201ddad 160
Kovalev_D 191:40028201ddad 161
Kovalev_D 112:4a96133a1311 162 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 196:f76dbc081e63 163 {
Kovalev_D 112:4a96133a1311 164 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 165 TempFaza = -4;
Kovalev_D 209:224e7331a061 166 Gyro.FrqPhaseEror=0;
Kovalev_D 209:224e7331a061 167 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0
Kovalev_D 209:224e7331a061 168 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {Gyro.FrqPhaseEror += Buff_Restored_sin [(CountV31 - Gyro.FrqPhase + CountFaza) & 0x1f];}
Kovalev_D 208:19150d2b528f 169 if(Gyro.RgConA&0x40)
Kovalev_D 208:19150d2b528f 170 { //12
Kovalev_D 209:224e7331a061 171 Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;
Kovalev_D 208:19150d2b528f 172 }
Kovalev_D 209:224e7331a061 173 // Gyro.FrqPhaseEror = TempFaza<<10;
Kovalev_D 189:8a16378724c4 174 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 189:8a16378724c4 175 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
Kovalev_D 208:19150d2b528f 176 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
igor_v 0:8ad47e2b6f00 177 }
igor_v 0:8ad47e2b6f00 178
igor_v 0:8ad47e2b6f00 179 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 180 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 181 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 182 void cheng(void)
Kovalev_D 99:3d8f206ceac2 183 {
Kovalev_D 107:4d178bcc9d8a 184 switch(CountV31) {
Kovalev_D 112:4a96133a1311 185 case 0:
Kovalev_D 196:f76dbc081e63 186 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 187 Time_vibro=0;
Kovalev_D 207:d1ce992f5d17 188 Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 112:4a96133a1311 189 break;
Kovalev_D 196:f76dbc081e63 190
Kovalev_D 112:4a96133a1311 191 case 16:
Kovalev_D 209:224e7331a061 192 // Gyro.Reper_Event=1;
Kovalev_D 112:4a96133a1311 193 Time_vibro=0;
Kovalev_D 191:40028201ddad 194 Gyro.VibroFrqRegulF=1; //
Kovalev_D 112:4a96133a1311 195 break;
Kovalev_D 207:d1ce992f5d17 196 }
Kovalev_D 191:40028201ddad 197 }
Kovalev_D 191:40028201ddad 198 void AllRegul (void)
Kovalev_D 191:40028201ddad 199 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 200
Kovalev_D 191:40028201ddad 201 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 202 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 197:7a05523bf588 203
Kovalev_D 197:7a05523bf588 204
Kovalev_D 197:7a05523bf588 205
Kovalev_D 197:7a05523bf588 206 /*if (Gyro.ADS_NewData == 1)
Kovalev_D 197:7a05523bf588 207 { Gyro.ADS_NewData = 0;
Kovalev_D 196:f76dbc081e63 208 switch(Gyro.LogPLC) {
Kovalev_D 196:f76dbc081e63 209 case 0: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 210 case 1: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 211 case 2: ShowMod(); break;
Kovalev_D 196:f76dbc081e63 212 }
Kovalev_D 197:7a05523bf588 213 }*/// был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
Kovalev_D 197:7a05523bf588 214
Kovalev_D 197:7a05523bf588 215
Kovalev_D 197:7a05523bf588 216
Kovalev_D 191:40028201ddad 217 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 209:224e7331a061 218 if (Gyro.VibroAMPRegulF == 1)
Kovalev_D 209:224e7331a061 219 {
Kovalev_D 209:224e7331a061 220 Gyro.VibroAMPRegulF = 0;
Kovalev_D 209:224e7331a061 221 VibroAMPRegul();
Kovalev_D 209:224e7331a061 222 if(!MODFlag)PLCRegul();
Kovalev_D 209:224e7331a061 223
Kovalev_D 209:224e7331a061 224 } // Регулеровка Амплитуды виброподвеса
Kovalev_D 209:224e7331a061 225 if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; OLDCalcAmpN();}
Kovalev_D 205:775d54fdf646 226 /* {
Kovalev_D 197:7a05523bf588 227 switch(Gyro.flag) {
Kovalev_D 197:7a05523bf588 228 case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break;
Kovalev_D 197:7a05523bf588 229 case 2: Gyro.AmpMin =3;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;Calc2AmpN(); break;
Kovalev_D 197:7a05523bf588 230 case 3: Gyro.AmpMin =1;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;CalcAmpD(); break;
Kovalev_D 205:775d54fdf646 231 }
Kovalev_D 205:775d54fdf646 232 } */// регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
Kovalev_D 203:3a6615de9581 233 //if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut();} // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно
Kovalev_D 196:f76dbc081e63 234 }