Dmitry Kovalev
/
LG2
fork
Fork of LG by
Diff: QEI.c
- Revision:
- 211:ac8251b067d2
- Parent:
- 210:b02fa166315d
- Child:
- 214:4c70e452c491
diff -r b02fa166315d -r ac8251b067d2 QEI.c --- a/QEI.c Mon Apr 24 05:43:54 2017 +0000 +++ b/QEI.c Mon Jul 03 05:50:08 2017 +0000 @@ -5,11 +5,13 @@ int FFF=0;//для заплатки unsigned int tempmod=64,tempmod2=1; -unsigned int tempReper; +unsigned int tempReper,tempReper2; int Pulse_8Point; int Pulse_16Point; int Pulse_32Point; int Pulse_64Point; +int Pulse_96Point; +int Pulse_128Point; int Pulse_16PointD; unsigned int CaunAddPlus =0;//счетчик ипульсов энкодера"+" за такт ВП unsigned int CaunAddMin =0;//счетчик ипульсов энкодера"-" за такт ВП @@ -18,7 +20,9 @@ int temp32=0; int Buff_1Point [512]; int Buff_32Point [32]; -int Buff_64Point [32]; +int Buff_64Point [32]; +int Buff_96Point [32]; +int Buff_128Point [32]; int Buff_16Point [32]; int Buff_16PointD [32]; int Buff_8Point [32]; @@ -39,8 +43,13 @@ temp32=0;*/ Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения. + + if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль + + /* sprintf((Time)," %d %d %d \r\n",Gyro.CaunPlusReperAdd,Gyro.CaunMinReperAdd, Dif_QEI); + WriteCon(Time);*/ Buff_1Point[CountV255] = Dif_QEI ; // накопление в буфер еденичных значений приращения по каждому такту. @@ -60,9 +69,16 @@ Buff_32Point[CountV31] = (Pulse_32Point ); Pulse_64Point += Buff_1Point[CountV255]; - Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за два 32 тактов - // Gyro.Cnt_Dif = (Pulse_64Point); + Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов Buff_64Point[CountV31] = (Pulse_64Point ); + + Pulse_96Point += Buff_1Point[CountV255]; + Pulse_96Point -= Buff_1Point[(CountV255-96) & 0xff]; // заполнение буфера накопленых приращений за 96 тактов + Buff_96Point[CountV31] = (Pulse_96Point ); + + Pulse_128Point += Buff_1Point[CountV255]; + Pulse_128Point -= Buff_1Point[(CountV255-128) & 0xff]; // заполнение буфера накопленых приращений за 128 тактов + Buff_128Point[CountV31] = (Pulse_128Point ); Pulse_16PointD += Buff_1Point[CountV255]; @@ -71,23 +87,33 @@ Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // Buff_16PointD[CountV31] = Pulse_16PointD ; - Buff_Restored_sin [CountV31] = (Buff_16PointD[CountV31]) - Buff_32Point[CountV31]; + Buff_Restored_sin [CountV31] = (Buff_16Point[CountV31])*2 - 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_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]; // счетчик отрицательных импульсов // расчет амплитуды*/ + */ + + // 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[CountV255]) > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255)&0xff]; // счетчик положительных импульсов + //else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды*/ + + + // if (LPC_QEI->STAT) Gyro.CaunMinReperAdd -= Dif_QEI; + // else Gyro.CaunPlusReperAdd += Dif_QEI; + + + + + switch(CountV31){ case 31: @@ -98,58 +124,51 @@ 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); */ + + sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper); + WriteCon(Time); */ break; } if(Gyro.FrqPhase==CountV31) { - Gyro.CaunMinReper = Gyro.CaunMinReperAdd; + //tempReper2=0; + /* Gyro.CaunMinReper = Gyro.CaunMinReperAdd; Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; Gyro.CaunMinReperAdd = 0; - Gyro.CaunPlusReperAdd = 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; - - + + + + if(Dif_QEI > 0) + { + if(tempReper) + { + Gyro.CaunMinReper = Gyro.CaunMinReperAdd; + Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; + Gyro.CaunMinReperAdd = 0; + Gyro.CaunPlusReperAdd = 0; + Gyro.Reper_Event=1; + // Gyro.CuruAngle += Gyro.CaunPlusReper-Gyro.CaunMinReper; + tempReper=0; + } + Gyro.CaunPlusReperAdd += Dif_QEI; + } + else + { + tempReper=1; + Gyro.CaunMinReperAdd -= Dif_QEI; + + } + + Gyro.CuruAngle += Buff_32Point [CountV31]; + Gyro.CuruAngle64 += Buff_64Point [CountV31]; + Gyro.CuruAngle96 += Buff_96Point [CountV31]; + Gyro.CuruAngle128 += Buff_128Point [CountV31]; + + - } - } - - 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; - }*/ + \ No newline at end of file