123
Fork of LG by
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?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 0:8ad47e2b6f00 | 1 | #include "Global.h" |
igor_v | 28: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 | 28: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 | } |