Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
Diff: QEI.c
- Revision:
- 42:6fc307c4963e
- Parent:
- 30:17c84ed091b3
- Child:
- 43:3f7c8a0ea59c
--- a/QEI.c Sat Feb 06 18:24:01 2016 +0000 +++ b/QEI.c Sun Feb 07 12:24:11 2016 +0000 @@ -5,8 +5,10 @@ int PulseHalf = 0; int CuruAngle = 0; int Dif_QEI; + int FFF=0;//для заплатки + int Pulse_8Point = 0; int Pulse_16Point = 0; int Pulse_32Point = 0; @@ -15,25 +17,128 @@ 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_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; // обработка прохода значения через ноль @@ -48,6 +153,7 @@ } else { Gyro.Cnt_Pls = -Dif_QEI; } + ////////// ???? Заплатка ////////// ???? Проблема начального заполнения буферов //////////////////////////////////////////////////////////////////////////////////////////////