Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@191:40028201ddad, 2016-08-29 (annotated)

Committer:
Kovalev_D
Date:
Mon Aug 29 11:58:52 2016 +0000
Revision:
191:40028201ddad
Parent:
190:289514f730ee
Child:
192:d32c8cf7bcd9
plc

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