Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
host/Source/App/QEI.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 "lpc17xx.h" |
Kovalev_D |
23:12e6183f04d4 | 2 | #include "QEI.h" |
Kovalev_D |
23:12e6183f04d4 | 3 | #include "vibro.h" |
Kovalev_D |
23:12e6183f04d4 | 4 | #include "MTimer.h" |
Kovalev_D |
23:12e6183f04d4 | 5 | |
Kovalev_D |
23:12e6183f04d4 | 6 | int Pulse_midl = 0; |
Kovalev_D |
23:12e6183f04d4 | 7 | int PulseHalf = 0; |
Kovalev_D |
23:12e6183f04d4 | 8 | int CuruAngle = 0; |
Kovalev_D |
23:12e6183f04d4 | 9 | int Dif_QEI; |
Kovalev_D |
23:12e6183f04d4 | 10 | int FFF=0;//äëÿ çàïëàòêè |
Kovalev_D |
23:12e6183f04d4 | 11 | |
Kovalev_D |
23:12e6183f04d4 | 12 | int Pulse_8Point = 0; |
Kovalev_D |
23:12e6183f04d4 | 13 | int Pulse_16Point = 0; |
Kovalev_D |
23:12e6183f04d4 | 14 | int Pulse_32Point = 0; |
Kovalev_D |
23:12e6183f04d4 | 15 | int Pulse_16PointD = 0; |
Kovalev_D |
23:12e6183f04d4 | 16 | unsigned int Iras=0,Temp_F_ras=0; |
Kovalev_D |
23:12e6183f04d4 | 17 | int yy = 0; |
Kovalev_D |
23:12e6183f04d4 | 18 | |
Kovalev_D |
23:12e6183f04d4 | 19 | |
Kovalev_D |
23:12e6183f04d4 | 20 | unsigned int Buff_32Point [256]; |
Kovalev_D |
23:12e6183f04d4 | 21 | unsigned int Buff_16Point [256]; |
Kovalev_D |
23:12e6183f04d4 | 22 | unsigned int Buff_16PointD [256]; |
Kovalev_D |
23:12e6183f04d4 | 23 | unsigned int Buff_8Point [256]; |
Kovalev_D |
23:12e6183f04d4 | 24 | unsigned int Buff_1Point [256]; |
Kovalev_D |
23:12e6183f04d4 | 25 | unsigned int Buff_Restored_sin [256]; |
Kovalev_D |
23:12e6183f04d4 | 26 | int unsigned Cur_QEI = 0, Last_QEI=0; |
Kovalev_D |
23:12e6183f04d4 | 27 | |
Kovalev_D |
23:12e6183f04d4 | 28 | |
Kovalev_D |
23:12e6183f04d4 | 29 | void D_QEI(void) |
Kovalev_D |
23:12e6183f04d4 | 30 | { |
Kovalev_D |
23:12e6183f04d4 | 31 | Dif_QEI=0; |
Kovalev_D |
23:12e6183f04d4 | 32 | |
Kovalev_D |
23:12e6183f04d4 | 33 | |
Kovalev_D |
23:12e6183f04d4 | 34 | Cur_QEI = LPC_QEI->POS & 0xFFFF; // ñ÷èòûâàíèå òåêóùåãî çíà÷åíèÿ ýíêîäåðà. |
Kovalev_D |
23:12e6183f04d4 | 35 | Dif_QEI = (Cur_QEI - Last_QEI); // ïîëó÷åíèå ïðèðàùåíèÿ.() |
Kovalev_D |
23:12e6183f04d4 | 36 | Last_QEI = Cur_QEI; // çàïèñü òåêóùåãî çíà÷åíèÿ ýíêîäåðà â ðåãèñòð ïðåäûäóùåãî çíà÷åíèÿ. |
Kovalev_D |
23:12e6183f04d4 | 37 | |
Kovalev_D |
23:12e6183f04d4 | 38 | |
Kovalev_D |
23:12e6183f04d4 | 39 | if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // îáðàáîòêà ïðîõîäà çíà÷åíèÿ ÷åðåç íîëü |
Kovalev_D |
23:12e6183f04d4 | 40 | if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // îáðàáîòêà ïðîõîäà çíà÷åíèÿ ÷åðåç íîëü |
Kovalev_D |
23:12e6183f04d4 | 41 | |
Kovalev_D |
23:12e6183f04d4 | 42 | |
Kovalev_D |
23:12e6183f04d4 | 43 | Buff_1Point[CountV255] = (unsigned int) (Dif_QEI + 0xffff);// íàêîïëåíèå â áóôåð åäåíè÷íûõ çíà÷åíèé ïðèðàùåíèÿ ïî êàæäîìó òàêòó. |
Kovalev_D |
23:12e6183f04d4 | 44 | |
Kovalev_D |
23:12e6183f04d4 | 45 | //////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 46 | /////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 47 | |
Kovalev_D |
23:12e6183f04d4 | 48 | if (LPC_QEI->STAT) //e. "+" direction //r. ñòàëè âðàùàòüñÿ â "+" ñòîðîíó |
Kovalev_D |
23:12e6183f04d4 | 49 | { |
Kovalev_D |
23:12e6183f04d4 | 50 | Main.Cnt_Mns = Dif_QEI; |
Kovalev_D |
23:12e6183f04d4 | 51 | } |
Kovalev_D |
23:12e6183f04d4 | 52 | else |
Kovalev_D |
23:12e6183f04d4 | 53 | { |
Kovalev_D |
23:12e6183f04d4 | 54 | Main.Cnt_Pls = -Dif_QEI; |
Kovalev_D |
23:12e6183f04d4 | 55 | } |
Kovalev_D |
23:12e6183f04d4 | 56 | ////////// ???? Çàïëàòêà |
Kovalev_D |
23:12e6183f04d4 | 57 | ////////// ???? Ïðîáëåìà íà÷àëüíîãî çàïîëíåíèÿ áóôåðîâ |
Kovalev_D |
23:12e6183f04d4 | 58 | ////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D |
23:12e6183f04d4 | 59 | if(FFF==1) // åñëè ïðîøëî 32 òàêòà âèáðî ïîäâåñà íà÷àòü çàïîëíÿòü îñòàëüíûå áóâåðû. |
Kovalev_D |
23:12e6183f04d4 | 60 | { |
Kovalev_D |
23:12e6183f04d4 | 61 | Pulse_8Point += Buff_1Point[CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 62 | Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 8 òàêòîâ |
Kovalev_D |
23:12e6183f04d4 | 63 | Buff_8Point[CountV255] = (unsigned int) (Pulse_8Point + 0xffff); |
Kovalev_D |
23:12e6183f04d4 | 64 | |
Kovalev_D |
23:12e6183f04d4 | 65 | Pulse_16Point += Buff_1Point[CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 66 | Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 16 òàêòîâ |
Kovalev_D |
23:12e6183f04d4 | 67 | Buff_16Point[CountV255] = (unsigned int) (Pulse_16Point + 0xffff); |
Kovalev_D |
23:12e6183f04d4 | 68 | |
Kovalev_D |
23:12e6183f04d4 | 69 | Pulse_32Point += Buff_1Point[CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 70 | Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 32 òàêòîâ |
Kovalev_D |
23:12e6183f04d4 | 71 | Main.Cnt_Dif = (Pulse_32Point+ 0xffff); |
Kovalev_D |
23:12e6183f04d4 | 72 | Buff_32Point[CountV255] = (unsigned int) (Pulse_32Point + 0xffff); |
Kovalev_D |
23:12e6183f04d4 | 73 | |
Kovalev_D |
23:12e6183f04d4 | 74 | Pulse_16PointD += Buff_1Point[CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 75 | Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 16 òàêòîâ Äâîéíûç |
Kovalev_D |
23:12e6183f04d4 | 76 | Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; // |
Kovalev_D |
23:12e6183f04d4 | 77 | Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // ç |
Kovalev_D |
23:12e6183f04d4 | 78 | |
Kovalev_D |
23:12e6183f04d4 | 79 | Buff_16PointD[CountV255] = (unsigned int) (Pulse_16PointD + 0xffff); |
Kovalev_D |
23:12e6183f04d4 | 80 | |
Kovalev_D |
23:12e6183f04d4 | 81 | |
Kovalev_D |
23:12e6183f04d4 | 82 | // Buff_Restored_sin [CountVf]= (unsigned int)( ((Buff_16Point[CountVf]*2)) - Buff_32Point[CountVf] ); |
Kovalev_D |
23:12e6183f04d4 | 83 | |
Kovalev_D |
23:12e6183f04d4 | 84 | Buff_Restored_sin [CountV255]= (unsigned int)( Buff_16PointD [ CountV255] + 65536 - Buff_32Point[CountV255] ); |
Kovalev_D |
23:12e6183f04d4 | 85 | |
Kovalev_D |
23:12e6183f04d4 | 86 | |
Kovalev_D |
23:12e6183f04d4 | 87 | |
Kovalev_D |
23:12e6183f04d4 | 88 | if(Buff_32Point[CountV255]>0) |
Kovalev_D |
23:12e6183f04d4 | 89 | { |
Kovalev_D |
23:12e6183f04d4 | 90 | Temp_F_ras += Buff_32Point[CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 91 | } |
Kovalev_D |
23:12e6183f04d4 | 92 | else if ((CountV255 & 0x1f)==0) |
Kovalev_D |
23:12e6183f04d4 | 93 | { |
Kovalev_D |
23:12e6183f04d4 | 94 | Main.F_ras=Temp_F_ras; |
Kovalev_D |
23:12e6183f04d4 | 95 | Temp_F_ras=0; |
Kovalev_D |
23:12e6183f04d4 | 96 | } |
Kovalev_D |
23:12e6183f04d4 | 97 | else |
Kovalev_D |
23:12e6183f04d4 | 98 | { |
Kovalev_D |
23:12e6183f04d4 | 99 | Temp_F_ras -= Buff_32Point[CountV255]; |
Kovalev_D |
23:12e6183f04d4 | 100 | } |
Kovalev_D |
23:12e6183f04d4 | 101 | |
Kovalev_D |
23:12e6183f04d4 | 102 | //unsigned int T_Vib; |
Kovalev_D |
23:12e6183f04d4 | 103 | |
Kovalev_D |
23:12e6183f04d4 | 104 | } |
Kovalev_D |
23:12e6183f04d4 | 105 | else if(CountV255 == 255) |
Kovalev_D |
23:12e6183f04d4 | 106 | { |
Kovalev_D |
23:12e6183f04d4 | 107 | FFF=1;//âðåìåííàÿ çàïëàòêà äëÿ ïàóçû ïåðåä çàïîëíåíèåì îñòàëüíûõ áóôåðîâ |
Kovalev_D |
23:12e6183f04d4 | 108 | for (yy = 0; yy < 256; yy++ ) |
Kovalev_D |
23:12e6183f04d4 | 109 | { |
Kovalev_D |
23:12e6183f04d4 | 110 | Buff_1Point [yy] = 0xffff; |
Kovalev_D |
23:12e6183f04d4 | 111 | Buff_16Point [yy] = 0xffff; |
Kovalev_D |
23:12e6183f04d4 | 112 | Buff_32Point [yy] = 0xffff; |
Kovalev_D |
23:12e6183f04d4 | 113 | Buff_16PointD [yy] = 0xffff; |
Kovalev_D |
23:12e6183f04d4 | 114 | } |
Kovalev_D |
23:12e6183f04d4 | 115 | |
Kovalev_D |
23:12e6183f04d4 | 116 | } |
Kovalev_D |
23:12e6183f04d4 | 117 | } |