123
Fork of LG by
QEI.c
- Committer:
- igor_v
- Date:
- 2016-02-07
- Revision:
- 42:6fc307c4963e
- Parent:
- 28:17c84ed091b3
- Child:
- 43:3f7c8a0ea59c
File content as of revision 42:6fc307c4963e:
#include "Global.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 Buff_32Point [32]; // давай сделаем буфера поменьше int Buff_16Point [32]; int Buff_16PointD [32]; int Buff_8Point [32]; int Buff_Restored_sin [32]; int Buff_1Point [256]; // этот на 256 int unsigned Cur_QEI = 0, Last_QEI=0; // ДАВАЙ попробуем сделать бушера в целых (знаковы) void D_QEI_Int(void) { 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] = Dif_QEI;// накопление в буфер еденичных значений приращения по каждому такту. //////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////// // зачем это. мож к этому воремени стали вращатся назад if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+" Gyro.Cnt_Mns = Dif_QEI; } else { Gyro.Cnt_Pls = -Dif_QEI; } ////////// ???? Заплатка ////////// ???? Проблема начального заполнения буферов ////////////////////////////////////////////////////////////////////////////////////////////// // если прошло 32 такта вибро подвеса начать заполнять остальные буверы. Pulse_8Point += Buff_1Point[CountV255]; Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов Buff_8Point[CountV31] =Pulse_8Point; Pulse_16Point += Buff_1Point[CountV255]; Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Buff_16Point[CountV31] = Pulse_16Point; Pulse_32Point += Buff_1Point[CountV255]; Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов Buff_32Point[CountV31] = Pulse_32Point; Gyro.Cnt_Dif = (Pulse_32Point); 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[CountV31] = Pulse_16PointD; // Buff_Restored_sin [CountVf]= (unsigned int)( ((Buff_16Point[CountVf]*2)) - Buff_32Point[CountVf] ); Buff_Restored_sin [CountV31]= Buff_16PointD [ CountV31] - Buff_32Point[CountV31]; if(Buff_32Point[CountV255]>0) { Temp_F_ras += Buff_32Point[CountV255]; } else if ((CountV255 & 0x1f)==0) { Gyro.F_ras=Temp_F_ras; Temp_F_ras=0; } else { Temp_F_ras -= Buff_32Point[CountV255]; } //unsigned int T_Vib; } 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. стали вращаться в "+" сторону Gyro.Cnt_Mns = Dif_QEI; } else { Gyro.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 тактов Gyro.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) { Gyro.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; } } }