Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
QEI.c
- Committer:
- Kovalev_D
- Date:
- 2017-04-24
- Revision:
- 210:b02fa166315d
- Parent:
- 209:224e7331a061
- Child:
- 211:ac8251b067d2
File content as of revision 210:b02fa166315d:
#include "Global.h" int Dif_QEI; int FFF=0;//для заплатки unsigned int tempmod=64,tempmod2=1; unsigned int tempReper; int Pulse_8Point; int Pulse_16Point; int Pulse_32Point; int Pulse_64Point; int Pulse_16PointD; unsigned int CaunAddPlus =0;//счетчик ипульсов энкодера"+" за такт ВП unsigned int CaunAddMin =0;//счетчик ипульсов энкодера"-" за такт ВП int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера int temp32=0; int Buff_1Point [512]; int Buff_32Point [32]; int Buff_64Point [32]; int Buff_16Point [32]; int Buff_16PointD [32]; int Buff_8Point [32]; int Buff_Restored_sin [32]; int Buff_Restored_sin2 [32]; void D_QEI(void) { Dif_QEI=0; Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера. //LPC_QEI->CON = 0xF; Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.() /* temp32 = Dif_QEI; Gyro.CuruAngle32 += temp32; temp32=0;*/ Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения. if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль Buff_1Point[CountV255] = Dif_QEI ; // накопление в буфер еденичных значений приращения по каждому такту. ////////////////////////////////////////////////////////////////////////////////////////////// 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 тактов 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); Buff_64Point[CountV31] = (Pulse_64Point ); 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 [CountV31] = (Buff_16PointD[CountV31]) - Buff_32Point[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 - 8/*Gyro.FrqPhase */)&0xff]; // счетчик положительных импульсов else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255 - 8/*Gyro.FrqPhase*/ )&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(Gyro.FrqPhase==CountV31) { Gyro.CaunMinReper = Gyro.CaunMinReperAdd; Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; Gyro.CaunMinReperAdd = 0; Gyro.CaunPlusReperAdd = 0; } 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 } /* void CurAngle(void) { for (int i = 0; i < 32; i++ ) { Gyro.CuruAngle += Buff_32Point[i]; } } void CurAngleLOG(void) { sprintf((Time)," %d %d \r\n", Gyro.tempdelta,Gyro.CuruAngle); WriteCon(Time); Gyro.CuruAngle=0; Gyro.tempdelta=0; }*/