![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
forkd
Fork of LG2 by
host/Source/App/vibro.c@23:12e6183f04d4, 2016-02-03 (annotated)
- Committer:
- Kovalev_D
- Date:
- Wed Feb 03 10:44:42 2016 +0300
- Revision:
- 23:12e6183f04d4
[thyz
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Kovalev_D |
23:12e6183f04d4 | 1 | #include "vibro.h" |
Kovalev_D |
23:12e6183f04d4 | 2 | #include "stdlib.h" |
Kovalev_D |
23:12e6183f04d4 | 3 | #include "lpc17xx.h" |
Kovalev_D |
23:12e6183f04d4 | 4 | #include "ctime" |
Kovalev_D |
23:12e6183f04d4 | 5 | #include "InputOutput.h" |
Kovalev_D |
23:12e6183f04d4 | 6 | #include "Parameters.h" |
Kovalev_D |
23:12e6183f04d4 | 7 | #include "CntrlGLD.h" |
Kovalev_D |
23:12e6183f04d4 | 8 | #include "CyclesSync.h" |
Kovalev_D |
23:12e6183f04d4 | 9 | #include "SIP.h" |
Kovalev_D |
23:12e6183f04d4 | 10 | //extern OUTPUT Output; |
Kovalev_D |
23:12e6183f04d4 | 11 | #include "QEI.h" |
Kovalev_D |
23:12e6183f04d4 | 12 | #include "Global.h" |
Kovalev_D |
23:12e6183f04d4 | 13 | #include "MTimer.h" |
Kovalev_D |
23:12e6183f04d4 | 14 | |
Kovalev_D |
23:12e6183f04d4 | 15 | struct MAIN Main; |
Kovalev_D |
23:12e6183f04d4 | 16 | |
Kovalev_D |
23:12e6183f04d4 | 17 | |
Kovalev_D |
23:12e6183f04d4 | 18 | volatile int V1 = 0 ; |
Kovalev_D |
23:12e6183f04d4 | 19 | volatile int Temp = 0 ; |
Kovalev_D |
23:12e6183f04d4 | 20 | volatile unsigned int Flag=0; |
Kovalev_D |
23:12e6183f04d4 | 21 | |
Kovalev_D |
23:12e6183f04d4 | 22 | unsigned int FrecTemp=0; |
Kovalev_D |
23:12e6183f04d4 | 23 | ///////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 24 | /////////////////////////èíèöèàëèçàöèÿ âèáðîïðèâîäà////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 25 | ///////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 26 | unsigned int init_Vibro () |
Kovalev_D |
23:12e6183f04d4 | 27 | { |
Kovalev_D |
23:12e6183f04d4 | 28 | Main.FrqRate=40; |
Kovalev_D |
23:12e6183f04d4 | 29 | Main.FrqMin=380; |
Kovalev_D |
23:12e6183f04d4 | 30 | Main.FrqHZ=449; |
Kovalev_D |
23:12e6183f04d4 | 31 | Main.Frq = Main.FrqHZ<<16; |
Kovalev_D |
23:12e6183f04d4 | 32 | |
Kovalev_D |
23:12e6183f04d4 | 33 | Main.AmpPer=20; |
Kovalev_D |
23:12e6183f04d4 | 34 | Main.AmpPerDel=10; |
Kovalev_D |
23:12e6183f04d4 | 35 | Main.Amp = 10*65535; |
Kovalev_D |
23:12e6183f04d4 | 36 | Main.AmpL = 20*650; |
Kovalev_D |
23:12e6183f04d4 | 37 | /* |
Kovalev_D |
23:12e6183f04d4 | 38 | Vib.AmpH=50*650; |
Kovalev_D |
23:12e6183f04d4 | 39 | // Vib.AmpT=600; |
Kovalev_D |
23:12e6183f04d4 | 40 | // Output.Str.T_Vibro=40; |
Kovalev_D |
23:12e6183f04d4 | 41 | //LPC_GPIO0->FIOSET |= (1<<5); |
Kovalev_D |
23:12e6183f04d4 | 42 | */ |
Kovalev_D |
23:12e6183f04d4 | 43 | return 0; |
Kovalev_D |
23:12e6183f04d4 | 44 | } |
Kovalev_D |
23:12e6183f04d4 | 45 | |
Kovalev_D |
23:12e6183f04d4 | 46 | |
Kovalev_D |
23:12e6183f04d4 | 47 | void VibroOut(void) |
Kovalev_D |
23:12e6183f04d4 | 48 | { |
Kovalev_D |
23:12e6183f04d4 | 49 | |
Kovalev_D |
23:12e6183f04d4 | 50 | |
Kovalev_D |
23:12e6183f04d4 | 51 | if(CountV31>=16) |
Kovalev_D |
23:12e6183f04d4 | 52 | { |
Kovalev_D |
23:12e6183f04d4 | 53 | if((Time_vibro>Main.AmpN1) && (Time_vibro<Main.AmpN2)) {ClrV2 /*LoopOn*/} |
Kovalev_D |
23:12e6183f04d4 | 54 | else{ SetV2 } |
Kovalev_D |
23:12e6183f04d4 | 55 | } |
Kovalev_D |
23:12e6183f04d4 | 56 | else |
Kovalev_D |
23:12e6183f04d4 | 57 | { |
Kovalev_D |
23:12e6183f04d4 | 58 | if((Time_vibro>Main.AmpN1)&&(Time_vibro<Main.AmpN2)) {ClrV1 /*LoopOff*/} |
Kovalev_D |
23:12e6183f04d4 | 59 | else{ SetV1 } |
Kovalev_D |
23:12e6183f04d4 | 60 | } |
Kovalev_D |
23:12e6183f04d4 | 61 | |
Kovalev_D |
23:12e6183f04d4 | 62 | |
Kovalev_D |
23:12e6183f04d4 | 63 | |
Kovalev_D |
23:12e6183f04d4 | 64 | } |
Kovalev_D |
23:12e6183f04d4 | 65 | |
Kovalev_D |
23:12e6183f04d4 | 66 | void CalcAmpN(void) |
Kovalev_D |
23:12e6183f04d4 | 67 | { |
Kovalev_D |
23:12e6183f04d4 | 68 | static int PeriodCount = 0,Tnoise; |
Kovalev_D |
23:12e6183f04d4 | 69 | |
Kovalev_D |
23:12e6183f04d4 | 70 | |
Kovalev_D |
23:12e6183f04d4 | 71 | |
Kovalev_D |
23:12e6183f04d4 | 72 | unsigned int Nmax=0; |
Kovalev_D |
23:12e6183f04d4 | 73 | |
Kovalev_D |
23:12e6183f04d4 | 74 | |
Kovalev_D |
23:12e6183f04d4 | 75 | if(PeriodCount>= Tnoise)//åñëè êîëè÷åñòâî çàõîäîâ â ïðåðûâàíèå áîëüøå ëèáî ðàâíî ÷àñòîòå îøóìëåíèÿ. |
Kovalev_D |
23:12e6183f04d4 | 76 | { |
Kovalev_D |
23:12e6183f04d4 | 77 | if (Flag==0)//ñåé÷àñ ìàëàÿ àìïëèòóäà? |
Kovalev_D |
23:12e6183f04d4 | 78 | { |
Kovalev_D |
23:12e6183f04d4 | 79 | |
Kovalev_D |
23:12e6183f04d4 | 80 | if(Main.AmpPer>90){Main.AmpPer=90;} |
Kovalev_D |
23:12e6183f04d4 | 81 | Nmax =(unsigned int)((100000/(Main.Frq>>16))-1); |
Kovalev_D |
23:12e6183f04d4 | 82 | Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer))/400); |
Kovalev_D |
23:12e6183f04d4 | 83 | Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1); |
Kovalev_D |
23:12e6183f04d4 | 84 | Flag=1; |
Kovalev_D |
23:12e6183f04d4 | 85 | |
Kovalev_D |
23:12e6183f04d4 | 86 | } |
Kovalev_D |
23:12e6183f04d4 | 87 | |
Kovalev_D |
23:12e6183f04d4 | 88 | else |
Kovalev_D |
23:12e6183f04d4 | 89 | { |
Kovalev_D |
23:12e6183f04d4 | 90 | if((Main.AmpPer+Main.AmpPerDel)>90){Main.AmpPer=90-Main.AmpPerDel;} |
Kovalev_D |
23:12e6183f04d4 | 91 | |
Kovalev_D |
23:12e6183f04d4 | 92 | Nmax =(unsigned int)((100000/(Main.Frq>>16))-1); |
Kovalev_D |
23:12e6183f04d4 | 93 | Main.AmpN1=(unsigned int)((Nmax*(100-Main.AmpPer+Main.AmpPerDel))/400); |
Kovalev_D |
23:12e6183f04d4 | 94 | Main.AmpN2=(unsigned int)((Nmax/2)-Main.AmpN1); |
Kovalev_D |
23:12e6183f04d4 | 95 | Flag=0; |
Kovalev_D |
23:12e6183f04d4 | 96 | } |
Kovalev_D |
23:12e6183f04d4 | 97 | Main.AmpMin =10; |
Kovalev_D |
23:12e6183f04d4 | 98 | Main.AmpTD =30; |
Kovalev_D |
23:12e6183f04d4 | 99 | //Tnoise=55; |
Kovalev_D |
23:12e6183f04d4 | 100 | srand(Global_Time); |
Kovalev_D |
23:12e6183f04d4 | 101 | Main.AmpT = (rand() % Main.AmpTD+Main.AmpMin);// ÎØÓÌËÅÍÈÅ amp |
Kovalev_D |
23:12e6183f04d4 | 102 | Tnoise=Main.AmpT; |
Kovalev_D |
23:12e6183f04d4 | 103 | //èçìåíåíèå ôëàãà ïðåäèäóùåé àìïëèòóäû |
Kovalev_D |
23:12e6183f04d4 | 104 | PeriodCount=0; |
Kovalev_D |
23:12e6183f04d4 | 105 | } |
Kovalev_D |
23:12e6183f04d4 | 106 | else{PeriodCount++;} |
Kovalev_D |
23:12e6183f04d4 | 107 | |
Kovalev_D |
23:12e6183f04d4 | 108 | |
Kovalev_D |
23:12e6183f04d4 | 109 | |
Kovalev_D |
23:12e6183f04d4 | 110 | } |
Kovalev_D |
23:12e6183f04d4 | 111 | |
Kovalev_D |
23:12e6183f04d4 | 112 | ////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 113 | /////////////////////////ôóíêöèÿ ðàáîòû âèáðîïðèâîäà////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 114 | ////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 115 | void cheng(void) |
Kovalev_D |
23:12e6183f04d4 | 116 | { |
Kovalev_D |
23:12e6183f04d4 | 117 | static int TempFaza, CountFaza; |
Kovalev_D |
23:12e6183f04d4 | 118 | |
Kovalev_D |
23:12e6183f04d4 | 119 | |
Kovalev_D |
23:12e6183f04d4 | 120 | //Noise(); |
Kovalev_D |
23:12e6183f04d4 | 121 | |
Kovalev_D |
23:12e6183f04d4 | 122 | //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; |
Kovalev_D |
23:12e6183f04d4 | 123 | /* |
Kovalev_D |
23:12e6183f04d4 | 124 | // CountV++; |
Kovalev_D |
23:12e6183f04d4 | 125 | // CountV &= 0x1f; |
Kovalev_D |
23:12e6183f04d4 | 126 | */ |
Kovalev_D |
23:12e6183f04d4 | 127 | if (Buff_Restored_sin [CountV255] > MaxAmp) |
Kovalev_D |
23:12e6183f04d4 | 128 | { |
Kovalev_D |
23:12e6183f04d4 | 129 | MaxAmp=Buff_Restored_sin [CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 130 | } |
Kovalev_D |
23:12e6183f04d4 | 131 | |
Kovalev_D |
23:12e6183f04d4 | 132 | |
Kovalev_D |
23:12e6183f04d4 | 133 | |
Kovalev_D |
23:12e6183f04d4 | 134 | |
Kovalev_D |
23:12e6183f04d4 | 135 | |
Kovalev_D |
23:12e6183f04d4 | 136 | switch(CountV31) |
Kovalev_D |
23:12e6183f04d4 | 137 | { |
Kovalev_D |
23:12e6183f04d4 | 138 | case 0: |
Kovalev_D |
23:12e6183f04d4 | 139 | CalcAmpN(); |
Kovalev_D |
23:12e6183f04d4 | 140 | //Vib.Frq = ((unsigned int) ((7680000*16/Output.Str.T_Vibro)))*16*16*16; |
Kovalev_D |
23:12e6183f04d4 | 141 | LPC_TIM1->MR0 =(unsigned int)(100000000/(Main.Frq>>11)); |
Kovalev_D |
23:12e6183f04d4 | 142 | Output.Str.T_Vibro=(unsigned int)((7680000*16/Main.Frq)*4096); |
Kovalev_D |
23:12e6183f04d4 | 143 | VibroPush(); |
Kovalev_D |
23:12e6183f04d4 | 144 | // LoopOn |
Kovalev_D |
23:12e6183f04d4 | 145 | // LPC_TIM1->MR0 =(unsigned int)(204800000000/Vib.Frq); |
Kovalev_D |
23:12e6183f04d4 | 146 | Time_vibro=0; |
Kovalev_D |
23:12e6183f04d4 | 147 | break; |
Kovalev_D |
23:12e6183f04d4 | 148 | case 10: |
Kovalev_D |
23:12e6183f04d4 | 149 | |
Kovalev_D |
23:12e6183f04d4 | 150 | // if (MaxAmp>(65535+400)) |
Kovalev_D |
23:12e6183f04d4 | 151 | // { |
Kovalev_D |
23:12e6183f04d4 | 152 | // Vib.Amp-=1000;// |
Kovalev_D |
23:12e6183f04d4 | 153 | // } |
Kovalev_D |
23:12e6183f04d4 | 154 | // else |
Kovalev_D |
23:12e6183f04d4 | 155 | // { |
Kovalev_D |
23:12e6183f04d4 | 156 | // Vib.Amp+=1000; |
Kovalev_D |
23:12e6183f04d4 | 157 | // } |
Kovalev_D |
23:12e6183f04d4 | 158 | |
Kovalev_D |
23:12e6183f04d4 | 159 | |
Kovalev_D |
23:12e6183f04d4 | 160 | |
Kovalev_D |
23:12e6183f04d4 | 161 | |
Kovalev_D |
23:12e6183f04d4 | 162 | Main.Amp -= (MaxAmp - 65536 - 300)*4; // ðàñ÷åò àìïëèòóäâ ñ ó÷åòîì ðàçíèöè |
Kovalev_D |
23:12e6183f04d4 | 163 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 164 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 165 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 166 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 167 | if(Main.Amp>2500000){Main.Amp=1200000;}// âðåìåííîå îãðàíè÷åíèå ðîñòà àìïëèòóäû â ñëó÷àå íåïîäîæåííîãî ãèðîñêîïà////////// |
Kovalev_D |
23:12e6183f04d4 | 168 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 169 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 170 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 171 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 172 | |
Kovalev_D |
23:12e6183f04d4 | 173 | |
Kovalev_D |
23:12e6183f04d4 | 174 | Main.AmpPer = Main.Amp>>16; |
Kovalev_D |
23:12e6183f04d4 | 175 | MaxAmp=0; |
Kovalev_D |
23:12e6183f04d4 | 176 | FrecTemp=Main.AmpPer;//äëÿ âûâîäà â òåðìèíàë |
Kovalev_D |
23:12e6183f04d4 | 177 | break; |
Kovalev_D |
23:12e6183f04d4 | 178 | case 16: |
Kovalev_D |
23:12e6183f04d4 | 179 | VibroPull(); |
Kovalev_D |
23:12e6183f04d4 | 180 | //SetV2 |
Kovalev_D |
23:12e6183f04d4 | 181 | // LoopOff |
Kovalev_D |
23:12e6183f04d4 | 182 | Time_vibro=0; |
Kovalev_D |
23:12e6183f04d4 | 183 | |
Kovalev_D |
23:12e6183f04d4 | 184 | |
Kovalev_D |
23:12e6183f04d4 | 185 | |
Kovalev_D |
23:12e6183f04d4 | 186 | |
Kovalev_D |
23:12e6183f04d4 | 187 | |
Kovalev_D |
23:12e6183f04d4 | 188 | |
Kovalev_D |
23:12e6183f04d4 | 189 | // ðàñ÷åò Ôàçû ñ ó÷åòîð ðàçíèöè |
Kovalev_D |
23:12e6183f04d4 | 190 | TempFaza = -4; |
Kovalev_D |
23:12e6183f04d4 | 191 | for (CountFaza = 0; CountFaza < 8; CountFaza++ ) |
Kovalev_D |
23:12e6183f04d4 | 192 | { |
Kovalev_D |
23:12e6183f04d4 | 193 | if (Buff_Restored_sin [(CountV255- 12 + CountFaza) & 0xff] > 65535) TempFaza++; |
Kovalev_D |
23:12e6183f04d4 | 194 | } // ðàñ÷åò Ôàçû ñ ó÷åòîð ðàçíèöè |
Kovalev_D |
23:12e6183f04d4 | 195 | Main.Frq -=TempFaza*1000; |
Kovalev_D |
23:12e6183f04d4 | 196 | |
Kovalev_D |
23:12e6183f04d4 | 197 | |
Kovalev_D |
23:12e6183f04d4 | 198 | |
Kovalev_D |
23:12e6183f04d4 | 199 | |
Kovalev_D |
23:12e6183f04d4 | 200 | // if (Buff_Restored_sin [(CountVf-8) & 0xff] > 65535) |
Kovalev_D |
23:12e6183f04d4 | 201 | // { |
Kovalev_D |
23:12e6183f04d4 | 202 | // Vib.Frq=Vib.Frq-10000; |
Kovalev_D |
23:12e6183f04d4 | 203 | // } |
Kovalev_D |
23:12e6183f04d4 | 204 | |
Kovalev_D |
23:12e6183f04d4 | 205 | // else |
Kovalev_D |
23:12e6183f04d4 | 206 | // { |
Kovalev_D |
23:12e6183f04d4 | 207 | // Vib.Frq=Vib.Frq+1000; |
Kovalev_D |
23:12e6183f04d4 | 208 | // } |
Kovalev_D |
23:12e6183f04d4 | 209 | break; |
Kovalev_D |
23:12e6183f04d4 | 210 | } |
Kovalev_D |
23:12e6183f04d4 | 211 | |
Kovalev_D |
23:12e6183f04d4 | 212 | |
Kovalev_D |
23:12e6183f04d4 | 213 | |
Kovalev_D |
23:12e6183f04d4 | 214 | |
Kovalev_D |
23:12e6183f04d4 | 215 | /* |
Kovalev_D |
23:12e6183f04d4 | 216 | if((CountV & 0x0f) == V1) |
Kovalev_D |
23:12e6183f04d4 | 217 | { |
Kovalev_D |
23:12e6183f04d4 | 218 | ClrV1 |
Kovalev_D |
23:12e6183f04d4 | 219 | ClrV2 |
Kovalev_D |
23:12e6183f04d4 | 220 | }*/ |
Kovalev_D |
23:12e6183f04d4 | 221 | } |