Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@208:19150d2b528f, 2017-02-07 (annotated)

Committer:
Kovalev_D
Date:
Tue Feb 07 10:11:35 2017 +0000
Revision:
208:19150d2b528f
Parent:
207:d1ce992f5d17
Child:
209:224e7331a061
465

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 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 205:775d54fdf646 47 /*
Kovalev_D 193:a0fe8bfc97e4 48 void Calc2AmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 49 {
Kovalev_D 193:a0fe8bfc97e4 50 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 51 static int PeriodCount = 0;
Kovalev_D 193:a0fe8bfc97e4 52 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 53 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 54 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 55 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 56
Kovalev_D 193:a0fe8bfc97e4 57 if(Gyro.AmpSC <55)countA++;
Kovalev_D 193:a0fe8bfc97e4 58 if(countA >2)
Kovalev_D 193:a0fe8bfc97e4 59 {
Kovalev_D 193:a0fe8bfc97e4 60 countA=0;
Kovalev_D 193:a0fe8bfc97e4 61 srand(Global_Time);
Kovalev_D 193:a0fe8bfc97e4 62 if(Cheng_AMP_Flag)
Kovalev_D 193:a0fe8bfc97e4 63 {
Kovalev_D 193:a0fe8bfc97e4 64 Cheng_AMP_Flag=0;
Kovalev_D 193:a0fe8bfc97e4 65 Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 66 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 67 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 68 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 69 }
Kovalev_D 193:a0fe8bfc97e4 70 else
Kovalev_D 193:a0fe8bfc97e4 71 {
Kovalev_D 193:a0fe8bfc97e4 72 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 73 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 74 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 75 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 76 }
Kovalev_D 193:a0fe8bfc97e4 77 } //8046
Kovalev_D 193:a0fe8bfc97e4 78
Kovalev_D 193:a0fe8bfc97e4 79 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 205:775d54fdf646 80 }*/
Kovalev_D 205:775d54fdf646 81 /*
Kovalev_D 190:289514f730ee 82 void CalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
igor_v 0:8ad47e2b6f00 83 {
Kovalev_D 191:40028201ddad 84 Gyro.AmpSC=0;
Kovalev_D 85:0466ee8cdfc8 85 static int PeriodCount = 0;
igor_v 21:bc8c1cec3da6 86 unsigned int Nmax=0;
Kovalev_D 191:40028201ddad 87 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 191:40028201ddad 88 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 191:40028201ddad 89 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 191:40028201ddad 90
Kovalev_D 196:f76dbc081e63 91 if(Gyro.AmpSC <5)countA++;
Kovalev_D 193:a0fe8bfc97e4 92 if(countA >3)
Kovalev_D 191:40028201ddad 93 {
Kovalev_D 191:40028201ddad 94 countA=0;
Kovalev_D 191:40028201ddad 95
Kovalev_D 191:40028201ddad 96 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 191:40028201ddad 97 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 191:40028201ddad 98 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 191:40028201ddad 99 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 100
Kovalev_D 191:40028201ddad 101 tempDP=Gyro.AmpPerDel;
Kovalev_D 191:40028201ddad 102 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 192:d32c8cf7bcd9 103 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 192:d32c8cf7bcd9 104 else Gyro.AmpPerDel = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 193:a0fe8bfc97e4 105 } //8046
Kovalev_D 193:a0fe8bfc97e4 106
Kovalev_D 193:a0fe8bfc97e4 107 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 108 }
Kovalev_D 193:a0fe8bfc97e4 109
Kovalev_D 205:775d54fdf646 110 */
Kovalev_D 193:a0fe8bfc97e4 111
Kovalev_D 207:d1ce992f5d17 112 /*
Kovalev_D 193:a0fe8bfc97e4 113 void CalcAmpD(void)
Kovalev_D 193:a0fe8bfc97e4 114 {
Kovalev_D 193:a0fe8bfc97e4 115 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 116 countA++;
Kovalev_D 193:a0fe8bfc97e4 117 if( countA>1)
Kovalev_D 193:a0fe8bfc97e4 118 {
Kovalev_D 193:a0fe8bfc97e4 119 countA=0;
Kovalev_D 193:a0fe8bfc97e4 120
Kovalev_D 193:a0fe8bfc97e4 121 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 122 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer-Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 123 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 124 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 125
Kovalev_D 193:a0fe8bfc97e4 126 // if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 127
Kovalev_D 193:a0fe8bfc97e4 128
Kovalev_D 193:a0fe8bfc97e4 129 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 130 case 0:
Kovalev_D 193:a0fe8bfc97e4 131 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 132 if (Gyro.AmpPerDel>10){Znak=1; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 133 break;
Kovalev_D 207:d1ce992f5d17 134
Kovalev_D 193:a0fe8bfc97e4 135 case 1:
Kovalev_D 193:a0fe8bfc97e4 136 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 137 if (Gyro.AmpPerDel<1)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 138 if (fnoize>6)Znak=2;
Kovalev_D 193:a0fe8bfc97e4 139 break;
Kovalev_D 196:f76dbc081e63 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 207:d1ce992f5d17 144 break;
Kovalev_D 207:d1ce992f5d17 145
Kovalev_D 193:a0fe8bfc97e4 146 case 3:
Kovalev_D 193:a0fe8bfc97e4 147 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 148 if (Gyro.AmpPerDel<1)Znak=2;
Kovalev_D 207:d1ce992f5d17 149 if (fnoize>12){Znak=4;}
Kovalev_D 193:a0fe8bfc97e4 150 break;
Kovalev_D 196:f76dbc081e63 151
Kovalev_D 193:a0fe8bfc97e4 152 case 4:
Kovalev_D 193:a0fe8bfc97e4 153 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 154 if (Gyro.AmpPerDel>15){Znak=5; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 155 break;
Kovalev_D 207:d1ce992f5d17 156
Kovalev_D 193:a0fe8bfc97e4 157 case 5:
Kovalev_D 193:a0fe8bfc97e4 158 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 159 if (Gyro.AmpPerDel<1)Znak=4;
Kovalev_D 207:d1ce992f5d17 160 if (fnoize>18){Znak=6;}
Kovalev_D 193:a0fe8bfc97e4 161 break;
Kovalev_D 196:f76dbc081e63 162
Kovalev_D 193:a0fe8bfc97e4 163 case 6:
Kovalev_D 193:a0fe8bfc97e4 164 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 165 if (Gyro.AmpPerDel>6){Znak=7;fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 166 break;
Kovalev_D 207:d1ce992f5d17 167
Kovalev_D 193:a0fe8bfc97e4 168 case 7:
Kovalev_D 193:a0fe8bfc97e4 169 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 170 if (Gyro.AmpPerDel<1)Znak=6;
Kovalev_D 193:a0fe8bfc97e4 171 if (fnoize>24){Znak=0;fnoize=0;}
Kovalev_D 193:a0fe8bfc97e4 172 break;
Kovalev_D 193:a0fe8bfc97e4 173 }
Kovalev_D 191:40028201ddad 174 }
Kovalev_D 193:a0fe8bfc97e4 175 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 176 }
Kovalev_D 207:d1ce992f5d17 177 */
Kovalev_D 193:a0fe8bfc97e4 178
Kovalev_D 207:d1ce992f5d17 179 void CalcAmpD(void)
Kovalev_D 208:19150d2b528f 180 {
Kovalev_D 208:19150d2b528f 181 // GyroP.Str.wall++;
Kovalev_D 208:19150d2b528f 182 // if(GyroP.Str.wall>16)
Kovalev_D 208:19150d2b528f 183 // {
Kovalev_D 208:19150d2b528f 184 // GyroP.Str.wall=0;
Kovalev_D 208:19150d2b528f 185 // klk++;
Kovalev_D 208:19150d2b528f 186 // if(klk>32) klk = 0;
Kovalev_D 208:19150d2b528f 187 // }
Kovalev_D 208:19150d2b528f 188 unsigned int Nmax=0;
Kovalev_D 208:19150d2b528f 189 Gyro.AmpPerDel = ModArrayTriangle[klk];
Kovalev_D 208:19150d2b528f 190
Kovalev_D 208:19150d2b528f 191 tempi++;
Kovalev_D 207:d1ce992f5d17 192 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 208:19150d2b528f 193 Gyro.AmpT = (rand() %8-4);// ОШУМЛЕНИЕ amp
Kovalev_D 207:d1ce992f5d17 194
Kovalev_D 207:d1ce992f5d17 195 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 207:d1ce992f5d17 196 Gyro.AmpN1=(unsigned int)((Nmax*((100-Gyro.AmpPer)+Gyro.AmpPerDel+Gyro.AmpT))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 207:d1ce992f5d17 197 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 198
Kovalev_D 207:d1ce992f5d17 199 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 208:19150d2b528f 200 // LPC_TIM1->MR0 +=(Gyro.AmpT<<5);
Kovalev_D 207:d1ce992f5d17 201 }
Kovalev_D 193:a0fe8bfc97e4 202
Kovalev_D 205:775d54fdf646 203 /*
Kovalev_D 193:a0fe8bfc97e4 204
Kovalev_D 193:a0fe8bfc97e4 205 void CalcAmpI(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 206 {
Kovalev_D 193:a0fe8bfc97e4 207 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 208 static int PeriodCount = 0 ;
Kovalev_D 193:a0fe8bfc97e4 209 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 210 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 211 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 212 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 213
Kovalev_D 193:a0fe8bfc97e4 214 countA++;
Kovalev_D 193:a0fe8bfc97e4 215 if( Gyro.AmpSC<60)
Kovalev_D 193:a0fe8bfc97e4 216 {
Kovalev_D 193:a0fe8bfc97e4 217 // countA=0;
Kovalev_D 193:a0fe8bfc97e4 218
Kovalev_D 193:a0fe8bfc97e4 219 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 220 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 221 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 222 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 223
Kovalev_D 193:a0fe8bfc97e4 224 // tempDP=Gyro.AmpPerDel;
Kovalev_D 193:a0fe8bfc97e4 225 // srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 193:a0fe8bfc97e4 226 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 227
Kovalev_D 193:a0fe8bfc97e4 228 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 229 case 0:
Kovalev_D 193:a0fe8bfc97e4 230 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 231 if (Gyro.AmpPerDel<1)Znak=1;
Kovalev_D 193:a0fe8bfc97e4 232 break;
Kovalev_D 193:a0fe8bfc97e4 233
Kovalev_D 193:a0fe8bfc97e4 234 case 1:
Kovalev_D 193:a0fe8bfc97e4 235 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 236 if (Gyro.AmpPerDel>13)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 237 break;
Kovalev_D 193:a0fe8bfc97e4 238 }
Kovalev_D 193:a0fe8bfc97e4 239
Kovalev_D 193:a0fe8bfc97e4 240 } //8046
Kovalev_D 193:a0fe8bfc97e4 241 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 242 }
Kovalev_D 205:775d54fdf646 243 */
Kovalev_D 193:a0fe8bfc97e4 244
Kovalev_D 207:d1ce992f5d17 245 int Mrand(void)
Kovalev_D 207:d1ce992f5d17 246 {
Kovalev_D 207:d1ce992f5d17 247 int b=0;
Kovalev_D 207:d1ce992f5d17 248 z=z*Gyro.AD_Slow;
Kovalev_D 207:d1ce992f5d17 249 b = ((z>>10) & 0xf)+20;
Kovalev_D 207:d1ce992f5d17 250 /* sprintf((Time),"%d\r\n", b);
Kovalev_D 207:d1ce992f5d17 251 WriteCon(Time);*/
Kovalev_D 207:d1ce992f5d17 252 return b;
Kovalev_D 207:d1ce992f5d17 253 }
Kovalev_D 193:a0fe8bfc97e4 254
Kovalev_D 193:a0fe8bfc97e4 255 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 207:d1ce992f5d17 256 {
Kovalev_D 207:d1ce992f5d17 257 static int PeriodCount = 0,Period=0;
Kovalev_D 204:e7e9762bf609 258 unsigned int Nmax=0, lowper=0;
Kovalev_D 208:19150d2b528f 259 Gyro.FrqHZ=Gyro.Frq>>16;
Kovalev_D 193:a0fe8bfc97e4 260 //расчет амплитуды относительно центральной точки
Kovalev_D 193:a0fe8bfc97e4 261 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 193:a0fe8bfc97e4 262 PeriodCount=0;//сбрасываем таймер
Kovalev_D 193:a0fe8bfc97e4 263
Kovalev_D 207:d1ce992f5d17 264 if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
Kovalev_D 193:a0fe8bfc97e4 265 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 266 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 267 }
Kovalev_D 207:d1ce992f5d17 268 Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1); //
Kovalev_D 203:3a6615de9581 269 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 270 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 208:19150d2b528f 271 if(Gyro.AmpPer<5)
Kovalev_D 203:3a6615de9581 272 {
Kovalev_D 203:3a6615de9581 273 lowper = Gyro.AmpN2-Gyro.AmpN1;
Kovalev_D 203:3a6615de9581 274 lowper=lowper/2;
Kovalev_D 203:3a6615de9581 275 Gyro.AmpN2= Gyro.AmpN2+lowper;
Kovalev_D 203:3a6615de9581 276 }
Kovalev_D 203:3a6615de9581 277 // Gyro.AmpN2=Gyro.AmpN1+2;
Kovalev_D 193:a0fe8bfc97e4 278 Cheng_AMP_Flag=1;
Kovalev_D 208:19150d2b528f 279 // Gyro.L_vibro=(103000/Nmax);
Kovalev_D 208:19150d2b528f 280 // Gyro.AmpPer=((((((Gyro.Frq>>12)*200)/16)*temp2)/7675000)/2)
Kovalev_D 208:19150d2b528f 281 Gyro.L_vibro=(((Gyro.AmpPer*7675000)/200)*32)/(Gyro.Frq>>12);
Kovalev_D 208:19150d2b528f 282 Gyro.L_vibro= Gyro.L_vibro*2;
Kovalev_D 193:a0fe8bfc97e4 283 }
Kovalev_D 193:a0fe8bfc97e4 284
Kovalev_D 208:19150d2b528f 285 else
Kovalev_D 208:19150d2b528f 286 {
Kovalev_D 193:a0fe8bfc97e4 287 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 288 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 289 }
Kovalev_D 208:19150d2b528f 290 if((Gyro.RgConA&0x40)&&(Gyro.RgConA&0x20))
Kovalev_D 208:19150d2b528f 291 {
Kovalev_D 207:d1ce992f5d17 292 Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 293 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 294 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 208:19150d2b528f 295 if(Gyro.AmpPer<5)
Kovalev_D 208:19150d2b528f 296 {
Kovalev_D 203:3a6615de9581 297 lowper = Gyro.AmpN2-Gyro.AmpN1;
Kovalev_D 203:3a6615de9581 298 lowper=lowper/2;
Kovalev_D 203:3a6615de9581 299 Gyro.AmpN2= Gyro.AmpN2+lowper+1;
Kovalev_D 208:19150d2b528f 300 }
Kovalev_D 203:3a6615de9581 301 }
Kovalev_D 203:3a6615de9581 302 // Gyro.AmpN2=Gyro.AmpN1+2;
Kovalev_D 193:a0fe8bfc97e4 303 Cheng_AMP_Flag=0;
Kovalev_D 208:19150d2b528f 304 //Gyro.L_vibro=(103000/Nmax);
Kovalev_D 208:19150d2b528f 305 Gyro.L_vibro=(((Gyro.AmpPer*7675000)/200)*32)/(Gyro.Frq>>12);
Kovalev_D 208:19150d2b528f 306 Gyro.L_vibro= Gyro.L_vibro*2;
Kovalev_D 207:d1ce992f5d17 307 }
Kovalev_D 207:d1ce992f5d17 308
Kovalev_D 207:d1ce992f5d17 309 // Period=Gyro.CuruAngle*101;
Kovalev_D 207:d1ce992f5d17 310
Kovalev_D 207:d1ce992f5d17 311 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 207:d1ce992f5d17 312 // srand(Mrand());
Kovalev_D 207:d1ce992f5d17 313 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 207:d1ce992f5d17 314
Kovalev_D 193:a0fe8bfc97e4 315 } else {
Kovalev_D 193:a0fe8bfc97e4 316 PeriodCount++;//таймер амплитуды
Kovalev_D 193:a0fe8bfc97e4 317 }
Kovalev_D 191:40028201ddad 318
Kovalev_D 208:19150d2b528f 319
Kovalev_D 192:d32c8cf7bcd9 320 }
Kovalev_D 207:d1ce992f5d17 321
Kovalev_D 207:d1ce992f5d17 322 /*дол лучших времен
Kovalev_D 207:d1ce992f5d17 323 unsigned long mwc()
Kovalev_D 207:d1ce992f5d17 324 {
Kovalev_D 207:d1ce992f5d17 325 static unsigned long x3456789,
Kovalev_D 207:d1ce992f5d17 326 y=362436069,
Kovalev_D 207:d1ce992f5d17 327 z=77465321,
Kovalev_D 207:d1ce992f5d17 328 c=13579;
Kovalev_D 207:d1ce992f5d17 329 unsigned long long t;
Kovalev_D 207:d1ce992f5d17 330 tС6905990LL*x+c;
Kovalev_D 207:d1ce992f5d17 331 x=y;
Kovalev_D 207:d1ce992f5d17 332 y=z;
Kovalev_D 207:d1ce992f5d17 333 c=(t>>32);
Kovalev_D 207:d1ce992f5d17 334 return z=(t&0xffffffff);
Kovalev_D 207:d1ce992f5d17 335 }
Kovalev_D 207:d1ce992f5d17 336 */
Kovalev_D 207:d1ce992f5d17 337
Kovalev_D 112:4a96133a1311 338 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 208:19150d2b528f 339 {
Kovalev_D 208:19150d2b528f 340 int temp=0;
Kovalev_D 189:8a16378724c4 341 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 342 CaunAddPlus = 0;
Kovalev_D 190:289514f730ee 343 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 344 CaunAddMin = 0;
Kovalev_D 208:19150d2b528f 345 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin;
Kovalev_D 208:19150d2b528f 346 Gyro.F_ras = Gyro.MaxAmp/32*Gyro.FrqHZ/22.5;
Kovalev_D 208:19150d2b528f 347 if(Gyro.RgConA&0x20)
Kovalev_D 208:19150d2b528f 348 {
Kovalev_D 208:19150d2b528f 349 //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 208:19150d2b528f 350 temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed));
Kovalev_D 208:19150d2b528f 351 temp=temp>>5;
Kovalev_D 208:19150d2b528f 352 Gyro.Amp -= temp; // расчет амплитуды ВП с учетом разници
Kovalev_D 208:19150d2b528f 353 if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 208:19150d2b528f 354 if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 208:19150d2b528f 355 Gyro.AmpPer = Gyro.Amp>>16; //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 208:19150d2b528f 356 }
Kovalev_D 208:19150d2b528f 357
Kovalev_D 208:19150d2b528f 358 /* sprintf((Time),"%d \r\n", ( Gyro.F_ras));
Kovalev_D 208:19150d2b528f 359 WriteCon(Time);*/
Kovalev_D 208:19150d2b528f 360 }
Kovalev_D 112:4a96133a1311 361
Kovalev_D 191:40028201ddad 362
Kovalev_D 191:40028201ddad 363
Kovalev_D 112:4a96133a1311 364 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 196:f76dbc081e63 365 {
Kovalev_D 112:4a96133a1311 366 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 367 TempFaza = -4;
Kovalev_D 208:19150d2b528f 368 if(Gyro.RgConA&0x40)
Kovalev_D 208:19150d2b528f 369 { //12
Kovalev_D 208:19150d2b528f 370 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 208:19150d2b528f 371 Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;
Kovalev_D 208:19150d2b528f 372 }
Kovalev_D 189:8a16378724c4 373 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 189:8a16378724c4 374 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
Kovalev_D 208:19150d2b528f 375 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
igor_v 0:8ad47e2b6f00 376 }
igor_v 0:8ad47e2b6f00 377
igor_v 0:8ad47e2b6f00 378 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 379 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 380 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 381 void cheng(void)
Kovalev_D 99:3d8f206ceac2 382 {
Kovalev_D 107:4d178bcc9d8a 383 switch(CountV31) {
Kovalev_D 112:4a96133a1311 384 case 0:
Kovalev_D 196:f76dbc081e63 385 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 386 Time_vibro=0;
Kovalev_D 207:d1ce992f5d17 387 Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 112:4a96133a1311 388 break;
Kovalev_D 196:f76dbc081e63 389
Kovalev_D 112:4a96133a1311 390 case 16:
Kovalev_D 197:7a05523bf588 391 Gyro.Reper_Event=1;
Kovalev_D 112:4a96133a1311 392 Time_vibro=0;
Kovalev_D 191:40028201ddad 393 Gyro.VibroFrqRegulF=1; //
Kovalev_D 112:4a96133a1311 394 break;
Kovalev_D 207:d1ce992f5d17 395 }
Kovalev_D 191:40028201ddad 396 }
Kovalev_D 191:40028201ddad 397 void AllRegul (void)
Kovalev_D 191:40028201ddad 398 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 399
Kovalev_D 191:40028201ddad 400 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 401 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 197:7a05523bf588 402
Kovalev_D 197:7a05523bf588 403
Kovalev_D 197:7a05523bf588 404
Kovalev_D 197:7a05523bf588 405 /*if (Gyro.ADS_NewData == 1)
Kovalev_D 197:7a05523bf588 406 { Gyro.ADS_NewData = 0;
Kovalev_D 196:f76dbc081e63 407 switch(Gyro.LogPLC) {
Kovalev_D 196:f76dbc081e63 408 case 0: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 409 case 1: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 410 case 2: ShowMod(); break;
Kovalev_D 196:f76dbc081e63 411 }
Kovalev_D 197:7a05523bf588 412 }*/// был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
Kovalev_D 197:7a05523bf588 413
Kovalev_D 197:7a05523bf588 414
Kovalev_D 197:7a05523bf588 415
Kovalev_D 191:40028201ddad 416 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 208:19150d2b528f 417 if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); PLCRegul(); } // Регулеровка Амплитуды виброподвеса
Kovalev_D 208:19150d2b528f 418 if (Gyro.VibroNoiseF == 1) {Gyro.VibroNoiseF = 0; /*CalcAmpD();*/ OLDCalcAmpN();}
Kovalev_D 205:775d54fdf646 419 /* {
Kovalev_D 197:7a05523bf588 420 switch(Gyro.flag) {
Kovalev_D 197:7a05523bf588 421 case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break;
Kovalev_D 197:7a05523bf588 422 case 2: Gyro.AmpMin =3;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;Calc2AmpN(); break;
Kovalev_D 197:7a05523bf588 423 case 3: Gyro.AmpMin =1;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;CalcAmpD(); break;
Kovalev_D 205:775d54fdf646 424 }
Kovalev_D 205:775d54fdf646 425 } */// регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
Kovalev_D 203:3a6615de9581 426 //if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut();} // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно
Kovalev_D 196:f76dbc081e63 427 }