Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

vibro.c@207:d1ce992f5d17, 2017-01-13 (annotated)

Committer:
Kovalev_D
Date:
Fri Jan 13 08:27:28 2017 +0000
Revision:
207:d1ce992f5d17
Parent:
205:775d54fdf646
Child:
208:19150d2b528f
4566

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 207:d1ce992f5d17 9 int i=16,tempi=0;
Kovalev_D 193:a0fe8bfc97e4 10 __irq void EINT3_IRQHandler()
Kovalev_D 205:775d54fdf646 11 { Gyro.EXT_Latch=1;
Kovalev_D 205:775d54fdf646 12 // Gyro.DeltaEXT_Event=1;
Kovalev_D 205:775d54fdf646 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 203:3a6615de9581 26 SetV1//установить в регистре PinReg бит "вибро 1" в "0"
Kovalev_D 190:289514f730ee 27 }
Kovalev_D 190:289514f730ee 28 else
Kovalev_D 190:289514f730ee 29 {
Kovalev_D 207:d1ce992f5d17 30 ClrV1 //установить в регистре 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 207:d1ce992f5d17 37 SetV2 //установить в регистре PinReg бит "вибро 2" в "0"
Kovalev_D 190:289514f730ee 38 }
Kovalev_D 190:289514f730ee 39 else
Kovalev_D 190:289514f730ee 40 {
Kovalev_D 203:3a6615de9581 41 ClrV2//установить в регистре 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 205:775d54fdf646 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 205:775d54fdf646 79 }*/
Kovalev_D 205:775d54fdf646 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 196:f76dbc081e63 90 if(Gyro.AmpSC <5)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 205:775d54fdf646 109 */
Kovalev_D 193:a0fe8bfc97e4 110
Kovalev_D 207:d1ce992f5d17 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 207:d1ce992f5d17 133
Kovalev_D 193:a0fe8bfc97e4 134 case 1:
Kovalev_D 193:a0fe8bfc97e4 135 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 136 if (Gyro.AmpPerDel<1)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 137 if (fnoize>6)Znak=2;
Kovalev_D 193:a0fe8bfc97e4 138 break;
Kovalev_D 196:f76dbc081e63 139
Kovalev_D 193:a0fe8bfc97e4 140 case 2:
Kovalev_D 193:a0fe8bfc97e4 141 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 142 if (Gyro.AmpPerDel>7){Znak=3; fnoize++;}
Kovalev_D 207:d1ce992f5d17 143 break;
Kovalev_D 207:d1ce992f5d17 144
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 207:d1ce992f5d17 148 if (fnoize>12){Znak=4;}
Kovalev_D 193:a0fe8bfc97e4 149 break;
Kovalev_D 196:f76dbc081e63 150
Kovalev_D 193:a0fe8bfc97e4 151 case 4:
Kovalev_D 193:a0fe8bfc97e4 152 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 153 if (Gyro.AmpPerDel>15){Znak=5; fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 154 break;
Kovalev_D 207:d1ce992f5d17 155
Kovalev_D 193:a0fe8bfc97e4 156 case 5:
Kovalev_D 193:a0fe8bfc97e4 157 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 158 if (Gyro.AmpPerDel<1)Znak=4;
Kovalev_D 207:d1ce992f5d17 159 if (fnoize>18){Znak=6;}
Kovalev_D 193:a0fe8bfc97e4 160 break;
Kovalev_D 196:f76dbc081e63 161
Kovalev_D 193:a0fe8bfc97e4 162 case 6:
Kovalev_D 193:a0fe8bfc97e4 163 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 164 if (Gyro.AmpPerDel>6){Znak=7;fnoize++;}
Kovalev_D 193:a0fe8bfc97e4 165 break;
Kovalev_D 207:d1ce992f5d17 166
Kovalev_D 193:a0fe8bfc97e4 167 case 7:
Kovalev_D 193:a0fe8bfc97e4 168 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 169 if (Gyro.AmpPerDel<1)Znak=6;
Kovalev_D 193:a0fe8bfc97e4 170 if (fnoize>24){Znak=0;fnoize=0;}
Kovalev_D 193:a0fe8bfc97e4 171 break;
Kovalev_D 193:a0fe8bfc97e4 172 }
Kovalev_D 191:40028201ddad 173 }
Kovalev_D 193:a0fe8bfc97e4 174 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 175 }
Kovalev_D 207:d1ce992f5d17 176 */
Kovalev_D 193:a0fe8bfc97e4 177
Kovalev_D 207:d1ce992f5d17 178 void CalcAmpD(void)
Kovalev_D 207:d1ce992f5d17 179 {
Kovalev_D 207:d1ce992f5d17 180 i++;
Kovalev_D 207:d1ce992f5d17 181 unsigned int Nmax=0;
Kovalev_D 207:d1ce992f5d17 182 Gyro.AmpPerDel = ModArrayTriangle[i];
Kovalev_D 207:d1ce992f5d17 183 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 207:d1ce992f5d17 184 Gyro.AmpT = (rand() % 2);// ОШУМЛЕНИЕ amp
Kovalev_D 207:d1ce992f5d17 185
Kovalev_D 207:d1ce992f5d17 186 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 207:d1ce992f5d17 187 Gyro.AmpN1=(unsigned int)((Nmax*((100-Gyro.AmpPer)+Gyro.AmpPerDel+Gyro.AmpT))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 207:d1ce992f5d17 188 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 207:d1ce992f5d17 189 if(i>32) i = 0;
Kovalev_D 193:a0fe8bfc97e4 190
Kovalev_D 207:d1ce992f5d17 191 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 207:d1ce992f5d17 192 }
Kovalev_D 193:a0fe8bfc97e4 193
Kovalev_D 205:775d54fdf646 194 /*
Kovalev_D 193:a0fe8bfc97e4 195
Kovalev_D 193:a0fe8bfc97e4 196 void CalcAmpI(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 193:a0fe8bfc97e4 197 {
Kovalev_D 193:a0fe8bfc97e4 198 Gyro.AmpSC=0;
Kovalev_D 193:a0fe8bfc97e4 199 static int PeriodCount = 0 ;
Kovalev_D 193:a0fe8bfc97e4 200 unsigned int Nmax=0;
Kovalev_D 193:a0fe8bfc97e4 201 Gyro.AmpSC = Gyro.MaxAmp - OldMaxAmp ;
Kovalev_D 193:a0fe8bfc97e4 202 if(Gyro.AmpSC<0) Gyro.AmpSC=Gyro.AmpSC*(-1);
Kovalev_D 193:a0fe8bfc97e4 203 OldMaxAmp=Gyro.MaxAmp;
Kovalev_D 193:a0fe8bfc97e4 204
Kovalev_D 193:a0fe8bfc97e4 205 countA++;
Kovalev_D 193:a0fe8bfc97e4 206 if( Gyro.AmpSC<60)
Kovalev_D 193:a0fe8bfc97e4 207 {
Kovalev_D 193:a0fe8bfc97e4 208 // countA=0;
Kovalev_D 193:a0fe8bfc97e4 209
Kovalev_D 193:a0fe8bfc97e4 210 Nmax = (unsigned int)((100000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 211 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/(Gyro.Frq>>16)); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 212 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 213 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 214
Kovalev_D 193:a0fe8bfc97e4 215 // tempDP=Gyro.AmpPerDel;
Kovalev_D 193:a0fe8bfc97e4 216 // srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 193:a0fe8bfc97e4 217 if(Gyro.flag==1) Gyro.AmpPerDel = 1;
Kovalev_D 193:a0fe8bfc97e4 218
Kovalev_D 193:a0fe8bfc97e4 219 switch(Znak) {
Kovalev_D 193:a0fe8bfc97e4 220 case 0:
Kovalev_D 193:a0fe8bfc97e4 221 Gyro.AmpPerDel--;
Kovalev_D 193:a0fe8bfc97e4 222 if (Gyro.AmpPerDel<1)Znak=1;
Kovalev_D 193:a0fe8bfc97e4 223 break;
Kovalev_D 193:a0fe8bfc97e4 224
Kovalev_D 193:a0fe8bfc97e4 225 case 1:
Kovalev_D 193:a0fe8bfc97e4 226 Gyro.AmpPerDel++;
Kovalev_D 193:a0fe8bfc97e4 227 if (Gyro.AmpPerDel>13)Znak=0;
Kovalev_D 193:a0fe8bfc97e4 228 break;
Kovalev_D 193:a0fe8bfc97e4 229 }
Kovalev_D 193:a0fe8bfc97e4 230
Kovalev_D 193:a0fe8bfc97e4 231 } //8046
Kovalev_D 193:a0fe8bfc97e4 232 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 193:a0fe8bfc97e4 233 }
Kovalev_D 205:775d54fdf646 234 */
Kovalev_D 193:a0fe8bfc97e4 235
Kovalev_D 207:d1ce992f5d17 236 int Mrand(void)
Kovalev_D 207:d1ce992f5d17 237 {
Kovalev_D 207:d1ce992f5d17 238 int b=0;
Kovalev_D 207:d1ce992f5d17 239 z=z*Gyro.AD_Slow;
Kovalev_D 207:d1ce992f5d17 240 b = ((z>>10) & 0xf)+20;
Kovalev_D 207:d1ce992f5d17 241 /* sprintf((Time),"%d\r\n", b);
Kovalev_D 207:d1ce992f5d17 242 WriteCon(Time);*/
Kovalev_D 207:d1ce992f5d17 243 return b;
Kovalev_D 207:d1ce992f5d17 244 }
Kovalev_D 193:a0fe8bfc97e4 245
Kovalev_D 193:a0fe8bfc97e4 246 void OLDCalcAmpN(void)//расчет точек старта и стопа импульса вибропривода и расчет частоты ошумления.
Kovalev_D 207:d1ce992f5d17 247 {
Kovalev_D 207:d1ce992f5d17 248 static int PeriodCount = 0,Period=0;
Kovalev_D 204:e7e9762bf609 249 unsigned int Nmax=0, lowper=0;
Kovalev_D 193:a0fe8bfc97e4 250
Kovalev_D 193:a0fe8bfc97e4 251 //расчет амплитуды относительно центральной точки
Kovalev_D 193:a0fe8bfc97e4 252 if(PeriodCount>= Gyro.AmpT) { //если количество заходов в прерывание больше либо равно частоте ошумления.
Kovalev_D 193:a0fe8bfc97e4 253 PeriodCount=0;//сбрасываем таймер
Kovalev_D 193:a0fe8bfc97e4 254
Kovalev_D 207:d1ce992f5d17 255 if (Cheng_AMP_Flag==0) { //сейчас малая амплитуда?
Kovalev_D 193:a0fe8bfc97e4 256 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 257 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 258 }
Kovalev_D 207:d1ce992f5d17 259 Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1); //
Kovalev_D 203:3a6615de9581 260 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer))/Gyro.FrqHZ); //левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 261 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 207:d1ce992f5d17 262 if(Gyro.AmpPer<20)
Kovalev_D 203:3a6615de9581 263 {
Kovalev_D 203:3a6615de9581 264 lowper = Gyro.AmpN2-Gyro.AmpN1;
Kovalev_D 203:3a6615de9581 265 lowper=lowper/2;
Kovalev_D 203:3a6615de9581 266 Gyro.AmpN2= Gyro.AmpN2+lowper;
Kovalev_D 203:3a6615de9581 267 }
Kovalev_D 203:3a6615de9581 268 // Gyro.AmpN2=Gyro.AmpN1+2;
Kovalev_D 193:a0fe8bfc97e4 269 Cheng_AMP_Flag=1;
Kovalev_D 193:a0fe8bfc97e4 270
Kovalev_D 193:a0fe8bfc97e4 271 }
Kovalev_D 193:a0fe8bfc97e4 272
Kovalev_D 193:a0fe8bfc97e4 273 else {
Kovalev_D 193:a0fe8bfc97e4 274 if((Gyro.AmpPer+Gyro.AmpPerDel)>90) {
Kovalev_D 193:a0fe8bfc97e4 275 Gyro.AmpPer=90-Gyro.AmpPerDel; //проверка верхней граници амплитуды
Kovalev_D 193:a0fe8bfc97e4 276 }
Kovalev_D 193:a0fe8bfc97e4 277
Kovalev_D 207:d1ce992f5d17 278 Nmax =(unsigned int)((103000/(Gyro.Frq>>16))-1);
Kovalev_D 193:a0fe8bfc97e4 279 Gyro.AmpN1=(unsigned int)((Nmax*(100-Gyro.AmpPer+Gyro.AmpPerDel))/Gyro.FrqHZ);//левая граница амплитуды
Kovalev_D 193:a0fe8bfc97e4 280 Gyro.AmpN2=(unsigned int)((Nmax/2)-Gyro.AmpN1); //правая граница амплитуды
Kovalev_D 207:d1ce992f5d17 281 if(Gyro.AmpPer<20)
Kovalev_D 203:3a6615de9581 282 {
Kovalev_D 203:3a6615de9581 283 lowper = Gyro.AmpN2-Gyro.AmpN1;
Kovalev_D 203:3a6615de9581 284 lowper=lowper/2;
Kovalev_D 203:3a6615de9581 285 Gyro.AmpN2= Gyro.AmpN2+lowper+1;
Kovalev_D 203:3a6615de9581 286 }
Kovalev_D 203:3a6615de9581 287 // Gyro.AmpN2=Gyro.AmpN1+2;
Kovalev_D 193:a0fe8bfc97e4 288 Cheng_AMP_Flag=0;
Kovalev_D 193:a0fe8bfc97e4 289
Kovalev_D 207:d1ce992f5d17 290 }
Kovalev_D 207:d1ce992f5d17 291
Kovalev_D 207:d1ce992f5d17 292 // Period=Gyro.CuruAngle*101;
Kovalev_D 207:d1ce992f5d17 293
Kovalev_D 207:d1ce992f5d17 294 srand(Global_Time);//инициализация функции rand() для получения новых случайных велечин.
Kovalev_D 207:d1ce992f5d17 295 // srand(Mrand());
Kovalev_D 207:d1ce992f5d17 296 Gyro.AmpT = (rand() % Gyro.AmpTD+Gyro.AmpMin);// ОШУМЛЕНИЕ amp
Kovalev_D 207:d1ce992f5d17 297
Kovalev_D 193:a0fe8bfc97e4 298 } else {
Kovalev_D 193:a0fe8bfc97e4 299 PeriodCount++;//таймер амплитуды
Kovalev_D 193:a0fe8bfc97e4 300 }
Kovalev_D 191:40028201ddad 301
Kovalev_D 207:d1ce992f5d17 302 LPC_TIM1->MR0 =(unsigned int)(103000000/((Gyro.Frq)>>11));//запись в таймер нового значение частоты вибро
Kovalev_D 192:d32c8cf7bcd9 303 }
Kovalev_D 207:d1ce992f5d17 304
Kovalev_D 207:d1ce992f5d17 305 /*дол лучших времен
Kovalev_D 207:d1ce992f5d17 306 unsigned long mwc()
Kovalev_D 207:d1ce992f5d17 307 {
Kovalev_D 207:d1ce992f5d17 308 static unsigned long x3456789,
Kovalev_D 207:d1ce992f5d17 309 y=362436069,
Kovalev_D 207:d1ce992f5d17 310 z=77465321,
Kovalev_D 207:d1ce992f5d17 311 c=13579;
Kovalev_D 207:d1ce992f5d17 312 unsigned long long t;
Kovalev_D 207:d1ce992f5d17 313 tС6905990LL*x+c;
Kovalev_D 207:d1ce992f5d17 314 x=y;
Kovalev_D 207:d1ce992f5d17 315 y=z;
Kovalev_D 207:d1ce992f5d17 316 c=(t>>32);
Kovalev_D 207:d1ce992f5d17 317 return z=(t&0xffffffff);
Kovalev_D 207:d1ce992f5d17 318 }
Kovalev_D 207:d1ce992f5d17 319 */
Kovalev_D 207:d1ce992f5d17 320
Kovalev_D 112:4a96133a1311 321 void VibroAMPRegul(void) //подстройка амплитуды ВП
Kovalev_D 112:4a96133a1311 322 {
Kovalev_D 189:8a16378724c4 323 Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 324 CaunAddPlus = 0;
Kovalev_D 190:289514f730ee 325 Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin
Kovalev_D 112:4a96133a1311 326 CaunAddMin = 0;
Kovalev_D 193:a0fe8bfc97e4 327 Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; //расчет максимальной амплитуды из востановленного синуса р-р.
Kovalev_D 207:d1ce992f5d17 328 Gyro.Amp -=(int)(((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed)); // расчет амплитуды ВП с учетом разници
Kovalev_D 207:d1ce992f5d17 329 //if((Gyro.Amp>>16) > Gyro.AmpPerMax) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 207:d1ce992f5d17 330 // if((Gyro.Amp>>16) < Gyro.AmpPerMin) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа//////////
Kovalev_D 191:40028201ddad 331 if(Gyro.RgConA&0x20) {Gyro.AmpPer = Gyro.Amp>>16;} //приведение амплитуды ВП к виду 0%-100%
Kovalev_D 207:d1ce992f5d17 332 /* sprintf((Time),"%d %d %d %d\r\n",Gyro.MaxAmp,(int)((((Gyro.MaxAmp - Gyro.AmpTarget) * Gyro.AmpSpeed)>>1)), Gyro.Amp>>16, Gyro.Amp);
Kovalev_D 207:d1ce992f5d17 333 WriteCon(Time);*/
Kovalev_D 196:f76dbc081e63 334
Kovalev_D 112:4a96133a1311 335 }
Kovalev_D 112:4a96133a1311 336
Kovalev_D 191:40028201ddad 337
Kovalev_D 191:40028201ddad 338
Kovalev_D 112:4a96133a1311 339 void VibroFrqRegul(void)// расчет Фазы с учетор разници(подстройка частоты)
Kovalev_D 196:f76dbc081e63 340 {
Kovalev_D 112:4a96133a1311 341 static int TempFaza, CountFaza;
Kovalev_D 112:4a96133a1311 342 TempFaza = -4;
Kovalev_D 191:40028201ddad 343 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 344 if(Gyro.RgConA&0x40) {Gyro.Frq += TempFaza*Gyro.FrqChengSpeed;}
Kovalev_D 189:8a16378724c4 345
Kovalev_D 189:8a16378724c4 346 if (Gyro.Frq < Gyro.FrqHZmin) Gyro.Frq=Gyro.FrqHZmin;//нижнее ограничение частоты
Kovalev_D 189:8a16378724c4 347 else if(Gyro.Frq > Gyro.FrqHZmax) Gyro.Frq=Gyro.FrqHZmax;//верхнее ограничение частоты
igor_v 0:8ad47e2b6f00 348 }
igor_v 0:8ad47e2b6f00 349
igor_v 0:8ad47e2b6f00 350 //////////////////////////////////////////////////////////////////////////////
Kovalev_D 190:289514f730ee 351 /////////////////////////основного 32 тактного цикла//////////////////////////
igor_v 0:8ad47e2b6f00 352 //////////////////////////////////////////////////////////////////////////////
igor_v 0:8ad47e2b6f00 353 void cheng(void)
Kovalev_D 99:3d8f206ceac2 354 {
Kovalev_D 107:4d178bcc9d8a 355 switch(CountV31) {
Kovalev_D 112:4a96133a1311 356 case 0:
Kovalev_D 207:d1ce992f5d17 357 /* tempi++;
Kovalev_D 207:d1ce992f5d17 358 if( tempi>31)
Kovalev_D 207:d1ce992f5d17 359 {*/
Kovalev_D 207:d1ce992f5d17 360 sprintf((Time),"%d %d %d %d %d\r\n", (Gyro.AmpPerDel+Gyro.AmpT)*50,Gyro.CuruAngle32*500,(Buff_32Point[CountV31])*500, CaunAddPlus-3200, CaunAddMin-3200);
Kovalev_D 207:d1ce992f5d17 361 WriteCon(Time);
Kovalev_D 207:d1ce992f5d17 362 Gyro.CuruAngle32=0;
Kovalev_D 207:d1ce992f5d17 363 tempi=0;
Kovalev_D 207:d1ce992f5d17 364 /* }
Kovalev_D 207:d1ce992f5d17 365
Kovalev_D 207:d1ce992f5d17 366 sprintf((Time)," %d %d\r\n",(Gyro.AmpPerDel+Gyro.AmpT),0);
Kovalev_D 207:d1ce992f5d17 367 WriteCon(Time);*/
Kovalev_D 207:d1ce992f5d17 368
Kovalev_D 196:f76dbc081e63 369 Gyro.VibroAMPRegulF=1;
Kovalev_D 112:4a96133a1311 370 Time_vibro=0;
Kovalev_D 207:d1ce992f5d17 371 Gyro.VibroNoiseF++;//расчет и установка нового заначения частоты ошумления и запись в таймер частоты ошумления.
Kovalev_D 207:d1ce992f5d17 372 /* sprintf((Time),"%d\r\n",Gyro.AmpT);
Kovalev_D 207:d1ce992f5d17 373 WriteCon(Time);*/
Kovalev_D 112:4a96133a1311 374 break;
Kovalev_D 196:f76dbc081e63 375
Kovalev_D 112:4a96133a1311 376 case 16:
Kovalev_D 197:7a05523bf588 377
Kovalev_D 197:7a05523bf588 378 Gyro.Reper_Event=1;
Kovalev_D 112:4a96133a1311 379 Time_vibro=0;
Kovalev_D 191:40028201ddad 380 Gyro.VibroFrqRegulF=1; //
Kovalev_D 207:d1ce992f5d17 381 /*sprintf((Time),"%d\r\n",Gyro.AmpT);
Kovalev_D 207:d1ce992f5d17 382 WriteCon(Time);*/
Kovalev_D 112:4a96133a1311 383 break;
Kovalev_D 207:d1ce992f5d17 384 }
Kovalev_D 191:40028201ddad 385 }
Kovalev_D 191:40028201ddad 386 void AllRegul (void)
Kovalev_D 191:40028201ddad 387 { ///////////////////////////контуры регулировки/////////////////////////////
Kovalev_D 191:40028201ddad 388
Kovalev_D 191:40028201ddad 389 if (Spi.ADC_NewData == 1) {ADS_Acum(); } // был приход новых данных по ацп сдесь сделать обработку информации и подготовку для выдачи делается 1 раз за вибро
Kovalev_D 191:40028201ddad 390 if (Gyro.ADF_NewData == 1) {Gyro.ADF_NewData = 0; } // был приход новых данных После быстрого фильтра AD
Kovalev_D 197:7a05523bf588 391
Kovalev_D 197:7a05523bf588 392
Kovalev_D 197:7a05523bf588 393
Kovalev_D 197:7a05523bf588 394 /*if (Gyro.ADS_NewData == 1)
Kovalev_D 197:7a05523bf588 395 { Gyro.ADS_NewData = 0;
Kovalev_D 196:f76dbc081e63 396 switch(Gyro.LogPLC) {
Kovalev_D 196:f76dbc081e63 397 case 0: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 398 case 1: PlcRegul(); break;
Kovalev_D 196:f76dbc081e63 399 case 2: ShowMod(); break;
Kovalev_D 196:f76dbc081e63 400 }
Kovalev_D 197:7a05523bf588 401 }*/// был приход новых данных После Медленного фильтра AD (гдето раз в 0.63 секунды )//регулировка периметра.
Kovalev_D 197:7a05523bf588 402
Kovalev_D 197:7a05523bf588 403
Kovalev_D 197:7a05523bf588 404
Kovalev_D 191:40028201ddad 405 if (Gyro.VibroFrqRegulF == 1) {Gyro.VibroFrqRegulF = 0; VibroFrqRegul(); } // Регулеровка частоты виброподвеса
Kovalev_D 191:40028201ddad 406 if (Gyro.VibroAMPRegulF == 1) {Gyro.VibroAMPRegulF = 0; VibroAMPRegul(); } // Регулеровка Амплитуды виброподвеса
Kovalev_D 207:d1ce992f5d17 407 if (Gyro.VibroNoiseF == 4) {Gyro.VibroNoiseF = 0; CalcAmpD();/*OLDCalcAmpN();*/}
Kovalev_D 205:775d54fdf646 408 /* {
Kovalev_D 197:7a05523bf588 409 switch(Gyro.flag) {
Kovalev_D 197:7a05523bf588 410 case 1: Gyro.VibroNoiseF = 0; OLDCalcAmpN(); break;
Kovalev_D 197:7a05523bf588 411 case 2: Gyro.AmpMin =3;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;Calc2AmpN(); break;
Kovalev_D 197:7a05523bf588 412 case 3: Gyro.AmpMin =1;Gyro.AmpTD =10;Gyro.VibroNoiseF = 0;CalcAmpD(); break;
Kovalev_D 205:775d54fdf646 413 }
Kovalev_D 205:775d54fdf646 414 } */// регулеровка ошумления, наверно нужно объеденить с регулеровкой ампитуды
Kovalev_D 203:3a6615de9581 415 //if (Gyro.VibroOutF == 1) {Gyro.VibroOutF = 0; VibroOut();} // установка ног в регисторе тоже подумать , зачем отделный флаг? наверно
Kovalev_D 196:f76dbc081e63 416 }