Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@193:a0fe8bfc97e4, 2016-09-15 (annotated)

Committer:
Kovalev_D
Date:
Thu Sep 15 11:09:00 2016 +0000
Revision:
193:a0fe8bfc97e4
Parent:
192:d32c8cf7bcd9
Child:
194:8f3cb37a5541
B_delta

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 193:a0fe8bfc97e4 18
igor_v 114:5cc38a53d8a7 19 void VibroOut(void) // выставка ног вибро
igor_v 0:8ad47e2b6f00 20 {
Kovalev_D 190:289514f730ee 21 if(CountV31>=16)
Kovalev_D 190:289514f730ee 22 {//первая нога вибро
Kovalev_D 89:a0d344db227e 23 // левая граница вЫкл вибро 1 > Time_vibro <ПРАВАЯ граница вЫкл вибро 1
Kovalev_D 190:289514f730ee 24 if((Time_vibro>Gyro.AmpN1) && (Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 25 {
Kovalev_D 115:e5a230e5af52 26 Gyro.PinReg &= ~PinRegBit_1V;//установить в регистре PinReg бит "вибро 1" в "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_1V;//установить в регистре PinReg бит "вибро 1" в "1"
igor_v 21:bc8c1cec3da6 31 }
Kovalev_D 85:0466ee8cdfc8 32
Kovalev_D 190:289514f730ee 33 }
Kovalev_D 190:289514f730ee 34 else {//вторая нога вибро
Kovalev_D 190:289514f730ee 35 if((Time_vibro>Gyro.AmpN1)&&(Time_vibro<Gyro.AmpN2))
Kovalev_D 190:289514f730ee 36 {
Kovalev_D 190:289514f730ee 37 Gyro.PinReg &= ~PinRegBit_2V; //установить в регистре PinReg бит "вибро 2" в "0"
Kovalev_D 190:289514f730ee 38 }
Kovalev_D 190:289514f730ee 39 else
Kovalev_D 190:289514f730ee 40 {
Kovalev_D 40:8a6494f61326 41 Gyro.PinReg |= PinRegBit_2V;//установить в регистре PinReg бит "вибро 2" в "1"
igor_v 21:bc8c1cec3da6 42 }
igor_v 21:bc8c1cec3da6 43 }
igor_v 0:8ad47e2b6f00 44 }
igor_v 0:8ad47e2b6f00 45
Kovalev_D 191:40028201ddad 46
Kovalev_D 193:a0fe8bfc97e4 47 void Calc2AmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 48 {
Kovalev_D 193:a0fe8bfc97e4 49 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 50 static int PeriodCount = 0;
Kovalev_D 193:a0fe8bfc97e4 51 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 52 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 53 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 54 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 55
Kovalev_D 193:a0fe8bfc97e4 56 if(Gyro.AmpSC <55)countA++;
Kovalev_D 193:a0fe8bfc97e4 57 if(countA >2)
Kovalev_D 193:a0fe8bfc97e4 58 {
Kovalev_D 193:a0fe8bfc97e4 59 countA=0;
Kovalev_D 193:a0fe8bfc97e4 60 srand(Global_Time);
Kovalev_D 193:a0fe8bfc97e4 61 if(Cheng_AMP_Flag)
Kovalev_D 193:a0fe8bfc97e4 62 {
Kovalev_D 193:a0fe8bfc97e4 63 Cheng_AMP_Flag=0;
Kovalev_D 193:a0fe8bfc97e4 64 Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 65 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 66 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 67 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 68 }
Kovalev_D 193:a0fe8bfc97e4 69 else
Kovalev_D 193:a0fe8bfc97e4 70 {
Kovalev_D 193:a0fe8bfc97e4 71 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 72 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 73 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 74 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 75 }
Kovalev_D 193:a0fe8bfc97e4 76 } //8046
Kovalev_D 193:a0fe8bfc97e4 77
Kovalev_D 193:a0fe8bfc97e4 78 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 79 }
Kovalev_D 191:40028201ddad 80
Kovalev_D 190:289514f730ee 81 void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
igor_v 0:8ad47e2b6f00 82 {
Kovalev_D 191:40028201ddad 83 Gyro.AmpSC=0;
Kovalev_D 85:0466ee8cdfc8 84 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 85 unsigned int Nmax=0;
Kovalev_D 191:40028201ddad 86 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 191:40028201ddad 87 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 191:40028201ddad 88 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 191:40028201ddad 89
Kovalev_D 191:40028201ddad 90 if(Gyro.AmpSC <20)countA++;
Kovalev_D 193:a0fe8bfc97e4 91 if(countA >3)
Kovalev_D 191:40028201ddad 92 {
Kovalev_D 191:40028201ddad 93 countA=0;
Kovalev_D 191:40028201ddad 94
Kovalev_D 191:40028201ddad 95 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 191:40028201ddad 96 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 191:40028201ddad 97 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 191:40028201ddad 98 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 99
Kovalev_D 191:40028201ddad 100 tempDP=Gyro.AmpPerDel;
Kovalev_D 191:40028201ddad 101 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 192:d32c8cf7bcd9 102 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 192:d32c8cf7bcd9 103 else Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 104 } //8046
Kovalev_D 193:a0fe8bfc97e4 105
Kovalev_D 193:a0fe8bfc97e4 106 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 107 }
Kovalev_D 193:a0fe8bfc97e4 108
Kovalev_D 193:a0fe8bfc97e4 109
Kovalev_D 193:a0fe8bfc97e4 110
Kovalev_D 193:a0fe8bfc97e4 111
Kovalev_D 193:a0fe8bfc97e4 112 void CalcAmpD(void)
Kovalev_D 193:a0fe8bfc97e4 113 {
Kovalev_D 193:a0fe8bfc97e4 114 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 115 countA++;
Kovalev_D 193:a0fe8bfc97e4 116 if( countA>1)
Kovalev_D 193:a0fe8bfc97e4 117 {
Kovalev_D 193:a0fe8bfc97e4 118 countA=0;
Kovalev_D 193:a0fe8bfc97e4 119
Kovalev_D 193:a0fe8bfc97e4 120 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 121 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 122 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 123 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 124
Kovalev_D 193:a0fe8bfc97e4 125 // if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 126
Kovalev_D 193:a0fe8bfc97e4 127
Kovalev_D 193:a0fe8bfc97e4 128 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 129 case 0:
Kovalev_D 193:a0fe8bfc97e4 130 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 131 if (Gyro.AmpPerDel>10){Znak=1; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 132 break;
Kovalev_D 193:a0fe8bfc97e4 133 case 1:
Kovalev_D 193:a0fe8bfc97e4 134 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 135 if (Gyro.AmpPerDel<1)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 136 if (fnoize>6)Znak=2;
Kovalev_D 193:a0fe8bfc97e4 137 break;
Kovalev_D 193:a0fe8bfc97e4 138
Kovalev_D 193:a0fe8bfc97e4 139
Kovalev_D 193:a0fe8bfc97e4 140
Kovalev_D 193:a0fe8bfc97e4 141 case 2:
Kovalev_D 193:a0fe8bfc97e4 142 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 143 if (Gyro.AmpPerDel>7){Znak=3; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 144 break;
Kovalev_D 193:a0fe8bfc97e4 145 case 3:
Kovalev_D 193:a0fe8bfc97e4 146 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 147 if (Gyro.AmpPerDel<1)Znak=2;
Kovalev_D 193:a0fe8bfc97e4 148 if (fnoize>12){Znak=4;/*fnoize=0;*/}
Kovalev_D 193:a0fe8bfc97e4 149 break;
Kovalev_D 193:a0fe8bfc97e4 150
Kovalev_D 193:a0fe8bfc97e4 151
Kovalev_D 193:a0fe8bfc97e4 152
Kovalev_D 193:a0fe8bfc97e4 153
Kovalev_D 193:a0fe8bfc97e4 154
Kovalev_D 193:a0fe8bfc97e4 155 case 4:
Kovalev_D 193:a0fe8bfc97e4 156 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 157 if (Gyro.AmpPerDel>15){Znak=5; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 158 break;
Kovalev_D 193:a0fe8bfc97e4 159 case 5:
Kovalev_D 193:a0fe8bfc97e4 160 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 161 if (Gyro.AmpPerDel<1)Znak=4;
Kovalev_D 193:a0fe8bfc97e4 162 if (fnoize>18){Znak=6;/*fnoize=0;*/}
Kovalev_D 193:a0fe8bfc97e4 163 break;
Kovalev_D 193:a0fe8bfc97e4 164
Kovalev_D 193:a0fe8bfc97e4 165
Kovalev_D 193:a0fe8bfc97e4 166
Kovalev_D 193:a0fe8bfc97e4 167
Kovalev_D 193:a0fe8bfc97e4 168
Kovalev_D 193:a0fe8bfc97e4 169
Kovalev_D 193:a0fe8bfc97e4 170 case 6:
Kovalev_D 193:a0fe8bfc97e4 171 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 172 if (Gyro.AmpPerDel>6){Znak=7;fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 173 break;
Kovalev_D 193:a0fe8bfc97e4 174 case 7:
Kovalev_D 193:a0fe8bfc97e4 175 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 176 if (Gyro.AmpPerDel<1)Znak=6;
Kovalev_D 193:a0fe8bfc97e4 177 if (fnoize>24){Znak=0;fnoize=0;}
Kovalev_D 193:a0fe8bfc97e4 178 break;
Kovalev_D 193:a0fe8bfc97e4 179 }
Kovalev_D 191:40028201ddad 180 }
Kovalev_D 193:a0fe8bfc97e4 181 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 182 }
Kovalev_D 193:a0fe8bfc97e4 183
Kovalev_D 193:a0fe8bfc97e4 184
Kovalev_D 193:a0fe8bfc97e4 185
Kovalev_D 193:a0fe8bfc97e4 186
Kovalev_D 193:a0fe8bfc97e4 187
Kovalev_D 193:a0fe8bfc97e4 188 void CalcAmpI(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 189 {
Kovalev_D 193:a0fe8bfc97e4 190 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 191 static int PeriodCount = 0 ;
Kovalev_D 193:a0fe8bfc97e4 192 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 193 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 194 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 195 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 196
Kovalev_D 193:a0fe8bfc97e4 197 countA++;
Kovalev_D 193:a0fe8bfc97e4 198 if( Gyro.AmpSC<60)
Kovalev_D 193:a0fe8bfc97e4 199 {
Kovalev_D 193:a0fe8bfc97e4 200 // countA=0;
Kovalev_D 193:a0fe8bfc97e4 201
Kovalev_D 193:a0fe8bfc97e4 202 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 203 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 204 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 205 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 206
Kovalev_D 193:a0fe8bfc97e4 207 // tempDP=Gyro.AmpPerDel;
Kovalev_D 193:a0fe8bfc97e4 208 // srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 193:a0fe8bfc97e4 209 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 210
Kovalev_D 193:a0fe8bfc97e4 211 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 212 case 0:
Kovalev_D 193:a0fe8bfc97e4 213 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 214 if (Gyro.AmpPerDel<1)Znak=1;
Kovalev_D 193:a0fe8bfc97e4 215 break;
Kovalev_D 193:a0fe8bfc97e4 216
Kovalev_D 193:a0fe8bfc97e4 217 case 1:
Kovalev_D 193:a0fe8bfc97e4 218 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 219 if (Gyro.AmpPerDel>13)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 220 break;
Kovalev_D 193:a0fe8bfc97e4 221 }
Kovalev_D 193:a0fe8bfc97e4 222
Kovalev_D 193:a0fe8bfc97e4 223 } //8046
Kovalev_D 193:a0fe8bfc97e4 224 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 225 }
Kovalev_D 193:a0fe8bfc97e4 226
Kovalev_D 193:a0fe8bfc97e4 227
Kovalev_D 193:a0fe8bfc97e4 228
Kovalev_D 193:a0fe8bfc97e4 229 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 230 {
Kovalev_D 193:a0fe8bfc97e4 231 static int PeriodCount = 0;
Kovalev_D 193:a0fe8bfc97e4 232 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 233
Kovalev_D 193:a0fe8bfc97e4 234 //расчет амплитуды относительно центральной точки
Kovalev_D 193:a0fe8bfc97e4 235 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 193:a0fe8bfc97e4 236 PeriodCount=0;//сбрасываем таймер
Kovalev_D 193:a0fe8bfc97e4 237
Kovalev_D 193:a0fe8bfc97e4 238 if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
Kovalev_D 193:a0fe8bfc97e4 239 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 240 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 241 }
Kovalev_D 193:a0fe8bfc97e4 242 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1); //
Kovalev_D 193:a0fe8bfc97e4 243 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 244 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 245 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 246
Kovalev_D 193:a0fe8bfc97e4 247 }
Kovalev_D 193:a0fe8bfc97e4 248
Kovalev_D 193:a0fe8bfc97e4 249 else {
Kovalev_D 193:a0fe8bfc97e4 250 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 251 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 252 }
Kovalev_D 193:a0fe8bfc97e4 253
Kovalev_D 193:a0fe8bfc97e4 254 Nmax =(unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 255 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 256 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 257 Cheng_AMP_Flag=0;
Kovalev_D 193:a0fe8bfc97e4 258
Kovalev_D 193:a0fe8bfc97e4 259 }
Kovalev_D 193:a0fe8bfc97e4 260 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 193:a0fe8bfc97e4 261 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 262
Kovalev_D 193:a0fe8bfc97e4 263 } else {
Kovalev_D 193:a0fe8bfc97e4 264 PeriodCount++;//таймер амплитуды
Kovalev_D 193:a0fe8bfc97e4 265 }
Kovalev_D 191:40028201ddad 266 switch( Gyro.StrayHZ_flag) {
Kovalev_D 193:a0fe8bfc97e4 267 case 0:
Kovalev_D 193:a0fe8bfc97e4 268 LPC_TIM1->MR0 =(unsigned int)(100000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 191:40028201ddad 269 break;
Kovalev_D 191:40028201ddad 270
Kovalev_D 191:40028201ddad 271 case 1:
Kovalev_D 193:a0fe8bfc97e4 272 LPC_TIM1->MR0 =(unsigned int)((100000000/(Gyro.Frq>>11))+ Gyro.StrayHZ);//запись в таймер нового значение частоты вибро + ошибка.
Kovalev_D 139:1716152517aa 273 break;
Kovalev_D 139:1716152517aa 274 }
Kovalev_D 192:d32c8cf7bcd9 275 }
Kovalev_D 112:4a96133a1311 276 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 112:4a96133a1311 277 {
Kovalev_D 189:8a16378724c4 278 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 279 CaunAddPlus = 0;
Kovalev_D 190:289514f730ee 280 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 281 CaunAddMin = 0;
Kovalev_D 193:a0fe8bfc97e4 282 /*Gyro.tempdelta=Gyro.CaunPlus - Gyro.CaunMin ;*/
Kovalev_D 193:a0fe8bfc97e4 283 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 191:40028201ddad 284 Gyro.Amp -= (Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed; // расчет амплитуды ВП с учетом разници
Kovalev_D 191:40028201ddad 285 if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 191:40028201ddad 286 if((Gyro.Amp>>16) > Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 191:40028201ddad 287 if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 191:40028201ddad 288 // Gyro.MaxAmp =0;
Kovalev_D 112:4a96133a1311 289 }
Kovalev_D 112:4a96133a1311 290
Kovalev_D 191:40028201ddad 291
Kovalev_D 191:40028201ddad 292
Kovalev_D 112:4a96133a1311 293 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 112:4a96133a1311 294 {
Kovalev_D 112:4a96133a1311 295 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 296 TempFaza = -4;
Kovalev_D 191:40028201ddad 297 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 298 if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;}
Kovalev_D 189:8a16378724c4 299
Kovalev_D 189:8a16378724c4 300 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 189:8a16378724c4 301 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
igor_v 0:8ad47e2b6f00 302 }
igor_v 0:8ad47e2b6f00 303
igor_v 0:8ad47e2b6f00 304 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 305 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 306 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 307 void cheng(void)
Kovalev_D 99:3d8f206ceac2 308 {
Kovalev_D 107:4d178bcc9d8a 309 switch(CountV31) {
Kovalev_D 112:4a96133a1311 310 case 0:
Kovalev_D 193:a0fe8bfc97e4 311 //CuruAngle();//СЧЕТ УГЛА ПО 32 ТОЧКАМ.
Kovalev_D 191:40028201ddad 312 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 313 Time_vibro=0;
Kovalev_D 191:40028201ddad 314 Gyro.VibroNoiseF=1;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 112:4a96133a1311 315 break;
Kovalev_D 112:4a96133a1311 316 case 16:
Kovalev_D 193:a0fe8bfc97e4 317 Gyro.Reper_Event=1;
Kovalev_D 112:4a96133a1311 318 Time_vibro=0;
Kovalev_D 191:40028201ddad 319 Gyro.VibroFrqRegulF=1; //
Kovalev_D 112:4a96133a1311 320 break;
Kovalev_D 108:030cdde08314 321
Kovalev_D 193:a0fe8bfc97e4 322 case 31:
Kovalev_D 193:a0fe8bfc97e4 323 //CuruAngleLOG();
Kovalev_D 193:a0fe8bfc97e4 324 break;
igor_v 21:bc8c1cec3da6 325 }
Kovalev_D 191:40028201ddad 326 }
Kovalev_D 191:40028201ddad 327 void AllRegul (void)
Kovalev_D 191:40028201ddad 328 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 329
Kovalev_D 191:40028201ddad 330 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 331 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 191:40028201ddad 332 if (Gyro.ADS_NewData == 1) {Gyro.ADS_NewData = 0; if(Gyro.ModJump == 3){ShowMod();} else {PlcRegul();}} // был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
Kovalev_D 191:40028201ddad 333 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 191:40028201ddad 334 if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса
Kovalev_D 192:d32c8cf7bcd9 335 if (Gyro.VibroNoiseF == 1)
Kovalev_D 192:d32c8cf7bcd9 336 {
Kovalev_D 193:a0fe8bfc97e4 337
Kovalev_D 192:d32c8cf7bcd9 338 if(Gyro.flag==2)
Kovalev_D 192:d32c8cf7bcd9 339 {
Kovalev_D 193:a0fe8bfc97e4 340 //Gyro.AmpPer=20;
Kovalev_D 193:a0fe8bfc97e4 341 Gyro.AmpPerDel=10;
Kovalev_D 193:a0fe8bfc97e4 342 Gyro.AmpMin =10;// минимальное значение AmpT;
Kovalev_D 193:a0fe8bfc97e4 343 Gyro.AmpTD =30;// максимальное значение AmpT; (AmpT частота ошумления)
Kovalev_D 193:a0fe8bfc97e4 344 OLDCalcAmpN();
Kovalev_D 193:a0fe8bfc97e4 345 Gyro.VibroNoiseF = 0;
Kovalev_D 192:d32c8cf7bcd9 346 }
Kovalev_D 193:a0fe8bfc97e4 347 else if(Gyro.flag==3)
Kovalev_D 193:a0fe8bfc97e4 348 {
Kovalev_D 193:a0fe8bfc97e4 349 Gyro.AmpMin =3;
Kovalev_D 193:a0fe8bfc97e4 350 Gyro.AmpTD =10;
Kovalev_D 193:a0fe8bfc97e4 351 Gyro.VibroNoiseF = 0;
Kovalev_D 193:a0fe8bfc97e4 352 Calc2AmpN();
Kovalev_D 193:a0fe8bfc97e4 353 }
Kovalev_D 192:d32c8cf7bcd9 354 else
Kovalev_D 192:d32c8cf7bcd9 355 {
Kovalev_D 193:a0fe8bfc97e4 356 Gyro.AmpMin =1;
Kovalev_D 193:a0fe8bfc97e4 357 Gyro.AmpTD =10;
Kovalev_D 192:d32c8cf7bcd9 358 Gyro.VibroNoiseF = 0;
Kovalev_D 193:a0fe8bfc97e4 359 //CalcAmpN();
Kovalev_D 193:a0fe8bfc97e4 360 Gyro.AmpTarget =15000;
Kovalev_D 193:a0fe8bfc97e4 361 CalcAmpI();
Kovalev_D 193:a0fe8bfc97e4 362 //CalcAmpD();
Kovalev_D 192:d32c8cf7bcd9 363 }
Kovalev_D 192:d32c8cf7bcd9 364 } // регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
Kovalev_D 191:40028201ddad 365 if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut(); } // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно
Kovalev_D 191:40028201ddad 366
Kovalev_D 191:40028201ddad 367 }