Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@192:d32c8cf7bcd9, 2016-09-01 (annotated)

Committer:
Kovalev_D
Date:
Thu Sep 01 08:23:30 2016 +0000
Revision:
192:d32c8cf7bcd9
Parent:
191:40028201ddad
Child:
193:a0fe8bfc97e4
ddd

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 191:40028201ddad 6 int Rate2VibFlag,countA=0,tempDP;
Kovalev_D 191:40028201ddad 7 unsigned int OldMaxAmp=0;
igor_v 114:5cc38a53d8a7 8 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 9 {
Kovalev_D 190:289514f730ee 10 if(CountV31>=16)
Kovalev_D 190:289514f730ee 11 {//первая нога вибро
Kovalev_D 89:a0d344db227e 12 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
Kovalev_D 190:289514f730ee 13 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 14 {
Kovalev_D 115:e5a230e5af52 15 Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "0"
Kovalev_D 190:289514f730ee 16 }
Kovalev_D 190:289514f730ee 17 else
Kovalev_D 190:289514f730ee 18 {
Kovalev_D 40:8a6494f61326 19 Gyro.PinReg |= PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 20 }
Kovalev_D 85:0466ee8cdfc8 21
Kovalev_D 190:289514f730ee 22 }
Kovalev_D 190:289514f730ee 23 else {//вторая нога вибро
Kovalev_D 190:289514f730ee 24 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 25 {
Kovalev_D 190:289514f730ee 26 Gyro.PinReg &= ~PinRegBit_2V; //установить в регистре PinReg бит "вибро 2" в "0"
Kovalev_D 190:289514f730ee 27 }
Kovalev_D 190:289514f730ee 28 else
Kovalev_D 190:289514f730ee 29 {
Kovalev_D 40:8a6494f61326 30 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 31 }
igor_v 21:bc8c1cec3da6 32 }
igor_v 0:8ad47e2b6f00 33 }
igor_v 0:8ad47e2b6f00 34
Kovalev_D 191:40028201ddad 35
Kovalev_D 191:40028201ddad 36
Kovalev_D 191:40028201ddad 37
Kovalev_D 190:289514f730ee 38 void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
igor_v 0:8ad47e2b6f00 39 {
Kovalev_D 191:40028201ddad 40 Gyro.AmpSC=0;
Kovalev_D 85:0466ee8cdfc8 41 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 42 unsigned int Nmax=0;
Kovalev_D 191:40028201ddad 43 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 191:40028201ddad 44 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 191:40028201ddad 45 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 191:40028201ddad 46
Kovalev_D 191:40028201ddad 47 if(Gyro.AmpSC <20)countA++;
Kovalev_D 191:40028201ddad 48 if(countA >5)
Kovalev_D 191:40028201ddad 49 {
Kovalev_D 191:40028201ddad 50 countA=0;
Kovalev_D 191:40028201ddad 51
Kovalev_D 191:40028201ddad 52 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 191:40028201ddad 53 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 191:40028201ddad 54 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 191:40028201ddad 55 Cheng_AMP_Flag=1;
Kovalev_D 191:40028201ddad 56 if(Gyro.LogHZ == 1) //Запись для Ориджина.
Kovalev_D 191:40028201ddad 57 {
Kovalev_D 191:40028201ddad 58 sprintf((Time),"%d \r\n",Gyro.AmpPerDel);
Kovalev_D 191:40028201ddad 59 WriteCon(Time);
Kovalev_D 191:40028201ddad 60 }
Kovalev_D 191:40028201ddad 61 tempDP=Gyro.AmpPerDel;
Kovalev_D 191:40028201ddad 62 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 192:d32c8cf7bcd9 63 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 192:d32c8cf7bcd9 64 else Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 191:40028201ddad 65 }
Kovalev_D 191:40028201ddad 66
Kovalev_D 191:40028201ddad 67 switch( Gyro.StrayHZ_flag) {
Kovalev_D 191:40028201ddad 68 case 0: //8046
Kovalev_D 191:40028201ddad 69 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 191:40028201ddad 70 break;
Kovalev_D 191:40028201ddad 71
Kovalev_D 191:40028201ddad 72 case 1:
Kovalev_D 191:40028201ddad 73 LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
Kovalev_D 191:40028201ddad 74 break;
Kovalev_D 191:40028201ddad 75 }
Kovalev_D 192:d32c8cf7bcd9 76 }
Kovalev_D 191:40028201ddad 77 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 191:40028201ddad 78 {
Kovalev_D 192:d32c8cf7bcd9 79 static int PeriodCount;
Kovalev_D 192:d32c8cf7bcd9 80 unsigned int Nmax;
Kovalev_D 192:d32c8cf7bcd9 81
Kovalev_D 85:0466ee8cdfc8 82 //расчет амплитуды относительно центральной точки
Kovalev_D 192:d32c8cf7bcd9 83 if(PeriodCount >= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 189:8a16378724c4 84 PeriodCount=0;//сбрасываем таймер
Kovalev_D 189:8a16378724c4 85
Kovalev_D 112:4a96133a1311 86 if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
Kovalev_D 192:d32c8cf7bcd9 87 /* if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды
Kovalev_D 189:8a16378724c4 88 else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды
Kovalev_D 192:d32c8cf7bcd9 89 */
Kovalev_D 189:8a16378724c4 90 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 169:140743e3bb96 91 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 189:8a16378724c4 92 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 189:8a16378724c4 93
Kovalev_D 169:140743e3bb96 94 Cheng_AMP_Flag=1;
igor_v 21:bc8c1cec3da6 95 }
igor_v 21:bc8c1cec3da6 96
igor_v 21:bc8c1cec3da6 97 else {
Kovalev_D 192:d32c8cf7bcd9 98 /* if ((Gyro.AmpPer+Gyro.AmpPerDel)> Gyro.AmpPerMax) {Gyro.AmpPer= Gyro.AmpPerMax - Gyro.AmpPerDel;} //проверка верхней граници амплитуды
Kovalev_D 192:d32c8cf7bcd9 99 else if((Gyro.AmpPer+Gyro.AmpPerDel)< Gyro.AmpPerMin) {Gyro.AmpPer= Gyro.AmpPerMin + Gyro.AmpPerDel;} //проверка нижней граници амплитуды*/
Kovalev_D 189:8a16378724c4 100
Kovalev_D 189:8a16378724c4 101 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 169:140743e3bb96 102 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16));//левая граница амплитуды
Kovalev_D 190:289514f730ee 103 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 189:8a16378724c4 104
Kovalev_D 169:140743e3bb96 105 Cheng_AMP_Flag=0;
igor_v 21:bc8c1cec3da6 106 }
Kovalev_D 85:0466ee8cdfc8 107 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
igor_v 30:17c84ed091b3 108 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 189:8a16378724c4 109 }
Kovalev_D 189:8a16378724c4 110 else {PeriodCount++;}
Kovalev_D 169:140743e3bb96 111
Kovalev_D 140:1fbf117fc120 112 switch( Gyro.StrayHZ_flag) {
Kovalev_D 169:140743e3bb96 113 case 0: //8046
Kovalev_D 169:140743e3bb96 114 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 139:1716152517aa 115 break;
Kovalev_D 139:1716152517aa 116
Kovalev_D 139:1716152517aa 117 case 1:
Kovalev_D 169:140743e3bb96 118 LPC_TIM1->MR0 =(unsigned int)((103000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
Kovalev_D 139:1716152517aa 119 break;
Kovalev_D 139:1716152517aa 120 }
Kovalev_D 192:d32c8cf7bcd9 121 }
Kovalev_D 112:4a96133a1311 122 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 112:4a96133a1311 123 {
Kovalev_D 189:8a16378724c4 124 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 125 CaunAddPlus = 0;
Kovalev_D 190:289514f730ee 126 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 127 CaunAddMin = 0;
Kovalev_D 112:4a96133a1311 128
Kovalev_D 190:289514f730ee 129 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 191:40028201ddad 130 Gyro.Amp -= (Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed; // расчет амплитуды ВП с учетом разници
Kovalev_D 191:40028201ddad 131 if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 191:40028201ddad 132 if((Gyro.Amp>>16) > Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 191:40028201ddad 133 if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 191:40028201ddad 134 // Gyro.MaxAmp =0;
Kovalev_D 112:4a96133a1311 135 }
Kovalev_D 112:4a96133a1311 136
Kovalev_D 191:40028201ddad 137
Kovalev_D 191:40028201ddad 138
Kovalev_D 112:4a96133a1311 139 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 112:4a96133a1311 140 {
Kovalev_D 112:4a96133a1311 141 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 142 TempFaza = -4;
Kovalev_D 191:40028201ddad 143 for (CountFaza = 0; CountFaza <8; CountFaza++ ) {if (Buff_Restored_sin [(CountV31 -12 + CountFaza) & 0xff] > 0 ) TempFaza++;} //резонанс когда CountV31 = 8 => Buff_Restored_sin = 0
Kovalev_D 191:40028201ddad 144 if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;}
Kovalev_D 189:8a16378724c4 145
Kovalev_D 189:8a16378724c4 146 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 189:8a16378724c4 147 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
igor_v 0:8ad47e2b6f00 148 }
igor_v 0:8ad47e2b6f00 149
igor_v 0:8ad47e2b6f00 150 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 151 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 152 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 153 void cheng(void)
Kovalev_D 99:3d8f206ceac2 154 {
Kovalev_D 107:4d178bcc9d8a 155 switch(CountV31) {
Kovalev_D 112:4a96133a1311 156 case 0:
Kovalev_D 191:40028201ddad 157 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 158 Time_vibro=0;
Kovalev_D 191:40028201ddad 159 Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 112:4a96133a1311 160 break;
Kovalev_D 112:4a96133a1311 161
Kovalev_D 112:4a96133a1311 162 case 16:
Kovalev_D 191:40028201ddad 163 /*if(Gyro.LogHZ == 1) //Запись для Ориджина.
Kovalev_D 191:40028201ddad 164 {
Kovalev_D 191:40028201ddad 165 sprintf((Time),"%d \r\n",Gyro.MaxAmp);
Kovalev_D 191:40028201ddad 166 WriteCon(Time);
Kovalev_D 191:40028201ddad 167 } */
Kovalev_D 112:4a96133a1311 168 Time_vibro=0;
Kovalev_D 191:40028201ddad 169 Gyro.VibroFrqRegulF=1; //
Kovalev_D 124:9ae09249f842 170 Gyro.Rate2_Event=1;
Kovalev_D 112:4a96133a1311 171 break;
Kovalev_D 108:030cdde08314 172
Kovalev_D 112:4a96133a1311 173 case 24:
Kovalev_D 191:40028201ddad 174
Kovalev_D 112:4a96133a1311 175 break;
igor_v 21:bc8c1cec3da6 176 }
Kovalev_D 191:40028201ddad 177 }
Kovalev_D 191:40028201ddad 178 void AllRegul (void)
Kovalev_D 191:40028201ddad 179 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 180
Kovalev_D 191:40028201ddad 181 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 182 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 191:40028201ddad 183 if (Gyro.ADS_NewData == 1) {Gyro.ADS_NewData = 0; if(Gyro.ModJump == 3){ShowMod();} else {PlcRegul();}} // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
Kovalev_D 191:40028201ddad 184 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 191:40028201ddad 185 if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса
Kovalev_D 192:d32c8cf7bcd9 186 if (Gyro.VibroNoiseF == 1)
Kovalev_D 192:d32c8cf7bcd9 187 {
Kovalev_D 192:d32c8cf7bcd9 188 if(Gyro.flag==2)
Kovalev_D 192:d32c8cf7bcd9 189 {
Kovalev_D 192:d32c8cf7bcd9 190 Gyro.AmpPerDel=2;
Kovalev_D 192:d32c8cf7bcd9 191 Gyro.AmpMin=20000;
Kovalev_D 192:d32c8cf7bcd9 192 Gyro.AmpTD=100;
Kovalev_D 192:d32c8cf7bcd9 193 OLDCalcAmpN();
Kovalev_D 192:d32c8cf7bcd9 194 }
Kovalev_D 192:d32c8cf7bcd9 195 else
Kovalev_D 192:d32c8cf7bcd9 196 {
Kovalev_D 192:d32c8cf7bcd9 197 Gyro.AmpMin =2;
Kovalev_D 192:d32c8cf7bcd9 198 Gyro.AmpTD =6;
Kovalev_D 192:d32c8cf7bcd9 199 Gyro.VibroNoiseF = 0;
Kovalev_D 192:d32c8cf7bcd9 200 CalcAmpN();
Kovalev_D 192:d32c8cf7bcd9 201 }
Kovalev_D 192:d32c8cf7bcd9 202 } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
Kovalev_D 191:40028201ddad 203 if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно
Kovalev_D 191:40028201ddad 204
Kovalev_D 191:40028201ddad 205 }