Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
Diff: QEI.c
- Revision:
- 21:bc8c1cec3da6
- Parent:
- 1:f2adcae3d304
- Child:
- 30:17c84ed091b3
diff -r 18e3fd7b92d0 -r bc8c1cec3da6 QEI.c --- a/QEI.c Tue Feb 02 17:14:25 2016 +0000 +++ b/QEI.c Wed Feb 03 07:19:30 2016 +0000 @@ -1,115 +1,103 @@ #include "Global.h" - int Pulse_midl = 0; - int PulseHalf = 0; - int CuruAngle = 0; - int Dif_QEI; -int FFF=0;//��� �������� + +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; +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; - void D_QEI(void) - { - Dif_QEI=0; - - - Cur_QEI = LPC_QEI->POS & 0xFFFF; // ���������� �������� �������� ��������. - Dif_QEI = (Cur_QEI - Last_QEI); // ��������� ����������.() - Last_QEI = Cur_QEI; // ������ �������� �������� �������� � ������� ����������� ��������. +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; // обработка прохода значения через ноль - 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); + 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_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]; // � + 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); - Buff_16PointD[CountV255] = (unsigned int) (Pulse_16PointD + 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] ); - + 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; + 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; - } + } 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; + } - } - } - + } +}