Dmitry Kovalev
/
LG2
fork
Fork of LG by
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?
User | Revision | Line number | New 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 | } |