123
Fork of LG by
vibro.c@21:bc8c1cec3da6, 2016-02-03 (annotated)
- Committer:
- igor_v
- Date:
- Wed Feb 03 07:19:30 2016 +0000
- Revision:
- 21:bc8c1cec3da6
- Parent:
- 0:8ad47e2b6f00
- Child:
- 28:17c84ed091b3
- Child:
- 31:1c9acd3b224d
?????????? ??????? ??????????, ???????? ??? ?????? ?????????? ??????????? ?????????.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 0:8ad47e2b6f00 | 1 | #include "Global.h" |
igor_v | 21:bc8c1cec3da6 | 2 | |
igor_v | 0:8ad47e2b6f00 | 3 | |
igor_v | 0:8ad47e2b6f00 | 4 | struct MAIN Main; |
igor_v | 0:8ad47e2b6f00 | 5 | |
igor_v | 0:8ad47e2b6f00 | 6 | |
igor_v | 0:8ad47e2b6f00 | 7 | volatile int V1 = 0 ; |
igor_v | 0:8ad47e2b6f00 | 8 | volatile int Temp = 0 ; |
igor_v | 0:8ad47e2b6f00 | 9 | volatile unsigned int Flag=0; |
igor_v | 0:8ad47e2b6f00 | 10 | |
igor_v | 0:8ad47e2b6f00 | 11 | unsigned int FrecTemp=0; |
igor_v | 0:8ad47e2b6f00 | 12 | ///////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 13 | /////////////////////////инициализация вибропривода////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 14 | ///////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 15 | unsigned int init_Vibro () |
igor_v | 21:bc8c1cec3da6 | 16 | { |
igor_v | 21:bc8c1cec3da6 | 17 | Main.FrqRate=40; |
igor_v | 21:bc8c1cec3da6 | 18 | Main.FrqMin=380; |
igor_v | 21:bc8c1cec3da6 | 19 | Main.FrqHZ=449; |
igor_v | 21:bc8c1cec3da6 | 20 | Main.Frq = Main.FrqHZ<<16; |
igor_v | 21:bc8c1cec3da6 | 21 | |
igor_v | 21:bc8c1cec3da6 | 22 | Main.AmpPer=20; |
igor_v | 21:bc8c1cec3da6 | 23 | Main.AmpPerDel=10; |
igor_v | 21:bc8c1cec3da6 | 24 | Main.Amp = 10*65535; |
igor_v | 21:bc8c1cec3da6 | 25 | Main.AmpL = 20*650; |
igor_v | 21:bc8c1cec3da6 | 26 | /* |
igor_v | 21:bc8c1cec3da6 | 27 | Vib.AmpH=50*650; |
igor_v | 21:bc8c1cec3da6 | 28 | // Vib.AmpT=600; |
igor_v | 21:bc8c1cec3da6 | 29 | // Output.Str.T_Vibro=40; |
igor_v | 21:bc8c1cec3da6 | 30 | //LPC_GPIO0->FIOSET |= (1<<5); |
igor_v | 21:bc8c1cec3da6 | 31 | */ |
igor_v | 21:bc8c1cec3da6 | 32 | return 0; |
igor_v | 21:bc8c1cec3da6 | 33 | } |
igor_v | 21:bc8c1cec3da6 | 34 | |
igor_v | 0:8ad47e2b6f00 | 35 | |
igor_v | 0:8ad47e2b6f00 | 36 | void VibroOut(void) |
igor_v | 0:8ad47e2b6f00 | 37 | { |
igor_v | 0:8ad47e2b6f00 | 38 | |
igor_v | 21:bc8c1cec3da6 | 39 | |
igor_v | 21:bc8c1cec3da6 | 40 | if(CountV31>=16) { |
igor_v | 21:bc8c1cec3da6 | 41 | if((Time_vibro>Main.AmpN1) && (Time_vibro<Main.AmpN2)) { |
igor_v | 21:bc8c1cec3da6 | 42 | ClrV2 /*LoopOn*/ |
igor_v | 21:bc8c1cec3da6 | 43 | } else { |
igor_v | 21:bc8c1cec3da6 | 44 | SetV2 |
igor_v | 21:bc8c1cec3da6 | 45 | } |
igor_v | 21:bc8c1cec3da6 | 46 | } else { |
igor_v | 21:bc8c1cec3da6 | 47 | if((Time_vibro>Main.AmpN1)&&(Time_vibro<Main.AmpN2)) { |
igor_v | 21:bc8c1cec3da6 | 48 | ClrV1 /*LoopOff*/ |
igor_v | 21:bc8c1cec3da6 | 49 | } else { |
igor_v | 21:bc8c1cec3da6 | 50 | SetV1 |
igor_v | 21:bc8c1cec3da6 | 51 | } |
igor_v | 21:bc8c1cec3da6 | 52 | } |
igor_v | 0:8ad47e2b6f00 | 53 | |
igor_v | 0:8ad47e2b6f00 | 54 | |
igor_v | 0:8ad47e2b6f00 | 55 | |
igor_v | 0:8ad47e2b6f00 | 56 | } |
igor_v | 0:8ad47e2b6f00 | 57 | |
igor_v | 0:8ad47e2b6f00 | 58 | void CalcAmpN(void) |
igor_v | 0:8ad47e2b6f00 | 59 | { |
igor_v | 21:bc8c1cec3da6 | 60 | static int PeriodCount = 0,Tnoise; |
igor_v | 21:bc8c1cec3da6 | 61 | |
igor_v | 21:bc8c1cec3da6 | 62 | |
igor_v | 21:bc8c1cec3da6 | 63 | |
igor_v | 21:bc8c1cec3da6 | 64 | unsigned int Nmax=0; |
igor_v | 21:bc8c1cec3da6 | 65 | |
igor_v | 21:bc8c1cec3da6 | 66 | |
igor_v | 21:bc8c1cec3da6 | 67 | if(PeriodCount>= Tnoise) { //если количество заходов в прерывание больше либо равно частоте ошумления. |
igor_v | 21:bc8c1cec3da6 | 68 | if (Flag==0) { //сейчас малая амплитуда? |
igor_v | 21:bc8c1cec3da6 | 69 | |
igor_v | 21:bc8c1cec3da6 | 70 | if(Main.AmpPer>90) { |
igor_v | 21:bc8c1cec3da6 | 71 | Main.AmpPer=90; |
igor_v | 21:bc8c1cec3da6 | 72 | } |
igor_v | 21:bc8c1cec3da6 | 73 | Nmax =(unsigned int)((100000/(Main.Frq>>16))-1); |
igor_v | 21:bc8c1cec3da6 | 74 | Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer))/400); |
igor_v | 21:bc8c1cec3da6 | 75 | Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1); |
igor_v | 21:bc8c1cec3da6 | 76 | Flag=1; |
igor_v | 21:bc8c1cec3da6 | 77 | |
igor_v | 21:bc8c1cec3da6 | 78 | } |
igor_v | 21:bc8c1cec3da6 | 79 | |
igor_v | 21:bc8c1cec3da6 | 80 | else { |
igor_v | 21:bc8c1cec3da6 | 81 | if((Main.AmpPer+Main.AmpPerDel)>90) { |
igor_v | 21:bc8c1cec3da6 | 82 | Main.AmpPer=90-Main.AmpPerDel; |
igor_v | 21:bc8c1cec3da6 | 83 | } |
igor_v | 21:bc8c1cec3da6 | 84 | |
igor_v | 21:bc8c1cec3da6 | 85 | Nmax =(unsigned int)((100000/(Main.Frq>>16))-1); |
igor_v | 21:bc8c1cec3da6 | 86 | Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer+Main.AmpPerDel))/400); |
igor_v | 21:bc8c1cec3da6 | 87 | Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1); |
igor_v | 21:bc8c1cec3da6 | 88 | Flag=0; |
igor_v | 21:bc8c1cec3da6 | 89 | } |
igor_v | 21:bc8c1cec3da6 | 90 | Main.AmpMin =10; |
igor_v | 21:bc8c1cec3da6 | 91 | Main.AmpTD =30; |
igor_v | 21:bc8c1cec3da6 | 92 | //Tnoise=55; |
igor_v | 21:bc8c1cec3da6 | 93 | srand(Global_Time); |
igor_v | 21:bc8c1cec3da6 | 94 | Main.AmpT = (rand() % Main.AmpTD+Main.AmpMin);// ОШУМЛЕНИЕ amp |
igor_v | 21:bc8c1cec3da6 | 95 | Tnoise=Main.AmpT; |
igor_v | 21:bc8c1cec3da6 | 96 | //изменение флага предидущей амплитуды |
igor_v | 21:bc8c1cec3da6 | 97 | PeriodCount=0; |
igor_v | 21:bc8c1cec3da6 | 98 | } else { |
igor_v | 21:bc8c1cec3da6 | 99 | PeriodCount++; |
igor_v | 21:bc8c1cec3da6 | 100 | } |
igor_v | 21:bc8c1cec3da6 | 101 | |
igor_v | 21:bc8c1cec3da6 | 102 | |
igor_v | 21:bc8c1cec3da6 | 103 | |
igor_v | 0:8ad47e2b6f00 | 104 | } |
igor_v | 0:8ad47e2b6f00 | 105 | |
igor_v | 0:8ad47e2b6f00 | 106 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 107 | /////////////////////////функция работы вибропривода////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 108 | ////////////////////////////////////////////////////////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 109 | void cheng(void) |
igor_v | 21:bc8c1cec3da6 | 110 | { |
igor_v | 21:bc8c1cec3da6 | 111 | static int TempFaza, CountFaza; |
igor_v | 0:8ad47e2b6f00 | 112 | |
igor_v | 0:8ad47e2b6f00 | 113 | |
igor_v | 21:bc8c1cec3da6 | 114 | //Noise(); |
igor_v | 0:8ad47e2b6f00 | 115 | |
igor_v | 21:bc8c1cec3da6 | 116 | //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; |
igor_v | 21:bc8c1cec3da6 | 117 | /* |
igor_v | 21:bc8c1cec3da6 | 118 | // CountV++; |
igor_v | 21:bc8c1cec3da6 | 119 | // CountV &= 0x1f; |
igor_v | 21:bc8c1cec3da6 | 120 | */ |
igor_v | 21:bc8c1cec3da6 | 121 | if (Buff_Restored_sin [CountV255] > MaxAmp) { |
igor_v | 21:bc8c1cec3da6 | 122 | MaxAmp=Buff_Restored_sin [CountV255]; |
igor_v | 21:bc8c1cec3da6 | 123 | } |
igor_v | 21:bc8c1cec3da6 | 124 | |
igor_v | 21:bc8c1cec3da6 | 125 | |
igor_v | 21:bc8c1cec3da6 | 126 | |
igor_v | 0:8ad47e2b6f00 | 127 | |
igor_v | 21:bc8c1cec3da6 | 128 | |
igor_v | 21:bc8c1cec3da6 | 129 | switch(CountV31) { |
igor_v | 21:bc8c1cec3da6 | 130 | case 0: |
igor_v | 21:bc8c1cec3da6 | 131 | CalcAmpN(); |
igor_v | 21:bc8c1cec3da6 | 132 | //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; |
igor_v | 21:bc8c1cec3da6 | 133 | LPC_TIM1->MR0 =(unsigned int)(100000000/(Main.Frq>>11)); |
igor_v | 21:bc8c1cec3da6 | 134 | Output.Str.T_Vibro=(unsigned int)((7680000*16/Main.Frq)*4096); |
igor_v | 21:bc8c1cec3da6 | 135 | VibroPush(); |
igor_v | 21:bc8c1cec3da6 | 136 | // LoopOn |
igor_v | 21:bc8c1cec3da6 | 137 | // LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq); |
igor_v | 21:bc8c1cec3da6 | 138 | Time_vibro=0; |
igor_v | 21:bc8c1cec3da6 | 139 | break; |
igor_v | 21:bc8c1cec3da6 | 140 | case 10: |
igor_v | 21:bc8c1cec3da6 | 141 | |
igor_v | 0:8ad47e2b6f00 | 142 | // if (MaxAmp>(65535+400)) |
igor_v | 0:8ad47e2b6f00 | 143 | // { |
igor_v | 0:8ad47e2b6f00 | 144 | // Vib.Amp-=1000;// |
igor_v | 0:8ad47e2b6f00 | 145 | // } |
igor_v | 0:8ad47e2b6f00 | 146 | // else |
igor_v | 0:8ad47e2b6f00 | 147 | // { |
igor_v | 0:8ad47e2b6f00 | 148 | // Vib.Amp+=1000; |
igor_v | 0:8ad47e2b6f00 | 149 | // } |
igor_v | 21:bc8c1cec3da6 | 150 | |
igor_v | 21:bc8c1cec3da6 | 151 | |
igor_v | 21:bc8c1cec3da6 | 152 | |
igor_v | 0:8ad47e2b6f00 | 153 | |
igor_v | 21:bc8c1cec3da6 | 154 | Main.Amp -= (MaxAmp - 65536 - 300)*4; // расчет амплитудв с учетом разници |
igor_v | 21:bc8c1cec3da6 | 155 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 156 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 157 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 158 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 159 | if(Main.Amp>2500000) { |
igor_v | 21:bc8c1cec3da6 | 160 | Main.Amp=1200000; // временное ограничение роста амплитуды в случае неподоженного гироскопа////////// |
igor_v | 21:bc8c1cec3da6 | 161 | } |
igor_v | 21:bc8c1cec3da6 | 162 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 163 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 164 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 165 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 0:8ad47e2b6f00 | 166 | |
igor_v | 0:8ad47e2b6f00 | 167 | |
igor_v | 21:bc8c1cec3da6 | 168 | Main.AmpPer = Main.Amp>>16; |
igor_v | 21:bc8c1cec3da6 | 169 | MaxAmp=0; |
igor_v | 21:bc8c1cec3da6 | 170 | FrecTemp=Main.AmpPer;//для вывода в терминал |
igor_v | 21:bc8c1cec3da6 | 171 | break; |
igor_v | 21:bc8c1cec3da6 | 172 | case 16: |
igor_v | 21:bc8c1cec3da6 | 173 | VibroPull(); |
igor_v | 21:bc8c1cec3da6 | 174 | //SetV2 |
igor_v | 21:bc8c1cec3da6 | 175 | // LoopOff |
igor_v | 21:bc8c1cec3da6 | 176 | Time_vibro=0; |
igor_v | 21:bc8c1cec3da6 | 177 | |
igor_v | 21:bc8c1cec3da6 | 178 | |
igor_v | 21:bc8c1cec3da6 | 179 | |
igor_v | 0:8ad47e2b6f00 | 180 | |
igor_v | 21:bc8c1cec3da6 | 181 | |
igor_v | 0:8ad47e2b6f00 | 182 | |
igor_v | 21:bc8c1cec3da6 | 183 | // расчет Фазы с учетор разници |
igor_v | 21:bc8c1cec3da6 | 184 | TempFaza = -4; |
igor_v | 21:bc8c1cec3da6 | 185 | for (CountFaza = 0; CountFaza < 8; CountFaza++ ) { |
igor_v | 21:bc8c1cec3da6 | 186 | if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++; |
igor_v | 21:bc8c1cec3da6 | 187 | } // расчет Фазы с учетор разници |
igor_v | 21:bc8c1cec3da6 | 188 | Main.Frq -=TempFaza*1000; |
igor_v | 21:bc8c1cec3da6 | 189 | |
igor_v | 21:bc8c1cec3da6 | 190 | |
igor_v | 21:bc8c1cec3da6 | 191 | |
igor_v | 21:bc8c1cec3da6 | 192 | |
igor_v | 0:8ad47e2b6f00 | 193 | // if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535) |
igor_v | 0:8ad47e2b6f00 | 194 | // { |
igor_v | 0:8ad47e2b6f00 | 195 | // Vib.Frq=Vib.Frq-10000; |
igor_v | 0:8ad47e2b6f00 | 196 | // } |
igor_v | 0:8ad47e2b6f00 | 197 | |
igor_v | 0:8ad47e2b6f00 | 198 | // else |
igor_v | 0:8ad47e2b6f00 | 199 | // { |
igor_v | 0:8ad47e2b6f00 | 200 | // Vib.Frq=Vib.Frq+1000; |
igor_v | 0:8ad47e2b6f00 | 201 | // } |
igor_v | 21:bc8c1cec3da6 | 202 | break; |
igor_v | 21:bc8c1cec3da6 | 203 | } |
igor_v | 21:bc8c1cec3da6 | 204 | |
igor_v | 0:8ad47e2b6f00 | 205 | |
igor_v | 0:8ad47e2b6f00 | 206 | |
igor_v | 0:8ad47e2b6f00 | 207 | |
igor_v | 21:bc8c1cec3da6 | 208 | /* |
igor_v | 21:bc8c1cec3da6 | 209 | if((CountV & 0x0f) == V1) |
igor_v | 21:bc8c1cec3da6 | 210 | { |
igor_v | 21:bc8c1cec3da6 | 211 | ClrV1 |
igor_v | 21:bc8c1cec3da6 | 212 | ClrV2 |
igor_v | 21:bc8c1cec3da6 | 213 | }*/ |
igor_v | 21:bc8c1cec3da6 | 214 | } |