Dmitry Kovalev
/
LG2
fork
Fork of LG by
Diff: QEI.c
- Revision:
- 209:224e7331a061
- Parent:
- 208:19150d2b528f
- Child:
- 210:b02fa166315d
diff -r 19150d2b528f -r 224e7331a061 QEI.c --- a/QEI.c Tue Feb 07 10:11:35 2017 +0000 +++ b/QEI.c Thu Apr 13 14:14:45 2017 +0000 @@ -5,7 +5,7 @@ int FFF=0;//для заплатки unsigned int tempmod=64,tempmod2=1; - +unsigned int tempReper; int Pulse_8Point; int Pulse_16Point; int Pulse_32Point; @@ -26,7 +26,7 @@ int Buff_Restored_sin2 [32]; void D_QEI(void) { - // LoopOn + Dif_QEI=0; Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера. @@ -58,10 +58,7 @@ Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов Gyro.Cnt_Dif = (Pulse_32Point); Buff_32Point[CountV31] = (Pulse_32Point ); - - - - + Pulse_64Point += Buff_1Point[CountV255]; Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за два 32 тактов // Gyro.Cnt_Dif = (Pulse_64Point); @@ -74,16 +71,56 @@ Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // Buff_16PointD[CountV31] = Pulse_16PointD ; - - Buff_Restored_sin [CountV31] = (Buff_16PointD[CountV31]) - Buff_32Point[CountV31]; - // Buff_Restored_sin2 [CountV31] = (Buff_16Point[CountV31]) - Buff_32Point[CountV31]; - if((Buff_Restored_sin [CountV31]) > 0) - { - CaunAddPlus += Buff_Restored_sin [CountV31]; - } // счетчик положительных импульсов - else {CaunAddMin -= Buff_Restored_sin [CountV31];} //счетчик отрицательных импульсов // расчет амплитуды - + + if((Buff_Restored_sin [CountV31]) > 0) CaunAddPlus += Buff_Restored_sin [CountV31]; // счетчик положительных импульсов + else CaunAddMin -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды + + + if((Buff_1Point[CountV31]) > 0) Gyro.CaunPlusRateAdd += Buff_1Point[CountV255]; // счетчик положительных импульсов + else Gyro.CaunMinRateAdd -= Buff_1Point[CountV255]; // счетчик отрицательных импульсов // расчет амплитуды + /* + if(Buff_Restored_sin [CountV31] > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик положительных импульсов + else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик отрицательных импульсов // расчет амплитуды + **/ + if(Buff_Restored_sin [CountV31]>0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255-7)&0xff]; // счетчик положительных импульсов + else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255-7)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды + + /*if((Buff_1Point[CountV31]) > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255-7)&0xff]; // счетчик положительных импульсов + else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255-7)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды*/ + + switch(CountV31){ + case 31: + Gyro.CaunMinRate = Gyro.CaunMinRateAdd; + Gyro.CaunPlusRate = Gyro.CaunPlusRateAdd; + Gyro.CaunMinRateAdd = 0; + Gyro.CaunPlusRateAdd = 0; + break; + case 10: + Gyro.CaunMinReper = Gyro.CaunMinReperAdd; + Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; + Gyro.CaunMinReperAdd = 0; + Gyro.CaunPlusReperAdd = 0; + + /* sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper); + WriteCon(Time); */ + break; + } + + if(Buff_Restored_sin [CountV31] > 0) + { + if(Gyro.Reper_Event)tempReper=0; + else tempReper=1; + } + else + { + if(tempReper) + { + tempReper=0; + Gyro.Reper_Event=1; + } + } + Gyro.CuruAngle += Buff_32Point [CountV31]; //LoopOff }