Dmitry Kovalev
/
LG
n
Fork of LG by
Diff: host/Source/App/QEI.c
- Revision:
- 23:12e6183f04d4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/host/Source/App/QEI.c Wed Feb 03 10:44:42 2016 +0300 @@ -0,0 +1,117 @@ +#include "lpc17xx.h" +#include "QEI.h" +#include "vibro.h" +#include "MTimer.h" + + int Pulse_midl = 0; + int PulseHalf = 0; + int CuruAngle = 0; + int Dif_QEI; +int FFF=0;//äëÿ çàïëàòêè + + int Pulse_8Point = 0; + int Pulse_16Point = 0; + int Pulse_32Point = 0; + int Pulse_16PointD = 0; + unsigned int Iras=0,Temp_F_ras=0; + int yy = 0; + + + unsigned int Buff_32Point [256]; + unsigned int Buff_16Point [256]; + unsigned int Buff_16PointD [256]; + unsigned int Buff_8Point [256]; + unsigned int Buff_1Point [256]; + unsigned int Buff_Restored_sin [256]; + int unsigned Cur_QEI = 0, Last_QEI=0; + + + void D_QEI(void) + { + Dif_QEI=0; + + + Cur_QEI = LPC_QEI->POS & 0xFFFF; // ñ÷èòûâàíèå òåêóùåãî çíà÷åíèÿ ýíêîäåðà. + Dif_QEI = (Cur_QEI - Last_QEI); // ïîëó÷åíèå ïðèðàùåíèÿ.() + Last_QEI = Cur_QEI; // çàïèñü òåêóùåãî çíà÷åíèÿ ýíêîäåðà â ðåãèñòð ïðåäûäóùåãî çíà÷åíèÿ. + + + if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // îáðàáîòêà ïðîõîäà çíà÷åíèÿ ÷åðåç íîëü + if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // îáðàáîòêà ïðîõîäà çíà÷åíèÿ ÷åðåç íîëü + + + Buff_1Point[CountV255] = (unsigned int) (Dif_QEI + 0xffff);// íàêîïëåíèå â áóôåð åäåíè÷íûõ çíà÷åíèé ïðèðàùåíèÿ ïî êàæäîìó òàêòó. + + //////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////// + + if (LPC_QEI->STAT) //e. "+" direction //r. ñòàëè âðàùàòüñÿ â "+" ñòîðîíó + { + Main.Cnt_Mns = Dif_QEI; + } + else + { + Main.Cnt_Pls = -Dif_QEI; + } + ////////// ???? Çàïëàòêà + ////////// ???? Ïðîáëåìà íà÷àëüíîãî çàïîëíåíèÿ áóôåðîâ + ////////////////////////////////////////////////////////////////////////////////////////////// + if(FFF==1) // åñëè ïðîøëî 32 òàêòà âèáðî ïîäâåñà íà÷àòü çàïîëíÿòü îñòàëüíûå áóâåðû. + { + Pulse_8Point += Buff_1Point[CountV255]; + Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 8 òàêòîâ + Buff_8Point[CountV255] = (unsigned int) (Pulse_8Point + 0xffff); + + Pulse_16Point += Buff_1Point[CountV255]; + Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 16 òàêòîâ + Buff_16Point[CountV255] = (unsigned int) (Pulse_16Point + 0xffff); + + Pulse_32Point += Buff_1Point[CountV255]; + Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 32 òàêòîâ + Main.Cnt_Dif = (Pulse_32Point+ 0xffff); + Buff_32Point[CountV255] = (unsigned int) (Pulse_32Point + 0xffff); + + Pulse_16PointD += Buff_1Point[CountV255]; + Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // çàïîëíåíèå áóôåðà íàêîïëåíûõ ïðèðàùåíèé çà 16 òàêòîâ Äâîéíûç + Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; // + Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // ç + + Buff_16PointD[CountV255] = (unsigned int) (Pulse_16PointD + 0xffff); + + +// Buff_Restored_sin [CountVf]= (unsigned int)( ((Buff_16Point[CountVf]*2)) - Buff_32Point[CountVf] ); + + Buff_Restored_sin [CountV255]= (unsigned int)( Buff_16PointD [ CountV255] + 65536 - Buff_32Point[CountV255] ); + + + + if(Buff_32Point[CountV255]>0) + { + Temp_F_ras += Buff_32Point[CountV255]; + } + else if ((CountV255 & 0x1f)==0) + { + Main.F_ras=Temp_F_ras; + Temp_F_ras=0; + } + else + { + Temp_F_ras -= Buff_32Point[CountV255]; + } + + //unsigned int T_Vib; + + } + else if(CountV255 == 255) + { + FFF=1;//âðåìåííàÿ çàïëàòêà äëÿ ïàóçû ïåðåä çàïîëíåíèåì îñòàëüíûõ áóôåðîâ + for (yy = 0; yy < 256; yy++ ) + { + Buff_1Point [yy] = 0xffff; + Buff_16Point [yy] = 0xffff; + Buff_32Point [yy] = 0xffff; + Buff_16PointD [yy] = 0xffff; + } + + } + }