Dmitry Kovalev
/
LGstaandart
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
LG2
by 
Dmitry Kovalev
Embed:
(wiki syntax)
Show/hide line numbers
QEI.c
00001 #include "Global.h" 00002 00003 int Dif_QEI; 00004 00005 int FFF=0;//для заплатки 00006 00007 unsigned int tempmod=64,tempmod2=1; 00008 unsigned int tempReper,tempReper2; 00009 int Pulse_8Point; 00010 int Pulse_16Point; 00011 int Pulse_32Point; 00012 int Pulse_64Point; 00013 int Pulse_96Point; 00014 int Pulse_128Point; 00015 int Pulse_16PointD; 00016 unsigned int CaunAddPlus =0;//счетчик ипульсов энкодера"+" за такт ВП 00017 unsigned int CaunAddMin =0;//счетчик ипульсов энкодера"-" за такт ВП 00018 00019 int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера 00020 int temp32=0; 00021 int Buff_1Point [512]; 00022 int Buff_32Point [32]; 00023 int Buff_64Point [32]; 00024 int Buff_96Point [32]; 00025 int Buff_128Point [32]; 00026 int Buff_16Point [32]; 00027 int Buff_16PointD [32]; 00028 int Buff_8Point [32]; 00029 int Buff_Restored_sin [32]; 00030 int Buff_Restored_sin2 [32]; 00031 void D_QEI(void) 00032 { 00033 00034 Dif_QEI=0; 00035 Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера. 00036 00037 00038 00039 //LPC_QEI->CON = 0xF; 00040 Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.() 00041 /* temp32 = Dif_QEI; 00042 Gyro.CuruAngle32 += temp32; 00043 temp32=0;*/ 00044 Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения. 00045 00046 00047 00048 if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль 00049 if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль 00050 00051 /* sprintf((Time)," %d %d %d \r\n",Gyro.CaunPlusReperAdd,Gyro.CaunMinReperAdd, Dif_QEI); 00052 WriteCon(Time);*/ 00053 Buff_1Point[CountV255] = Dif_QEI ; // накопление в буфер еденичных значений приращения по каждому такту. 00054 00055 00056 00057 ////////////////////////////////////////////////////////////////////////////////////////////// 00058 Pulse_8Point += Buff_1Point[CountV255]; 00059 Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов 00060 Buff_8Point[CountV31] = (Pulse_8Point); 00061 00062 Pulse_16Point += Buff_1Point[CountV255]; 00063 Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов 00064 Buff_16Point[CountV31] = (Pulse_16Point ); 00065 00066 Pulse_32Point += Buff_1Point[CountV255]; 00067 Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов 00068 Gyro.Cnt_Dif = (Pulse_32Point); 00069 Buff_32Point[CountV31] = (Pulse_32Point ); 00070 00071 Pulse_64Point += Buff_1Point[CountV255]; 00072 Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов 00073 Buff_64Point[CountV31] = (Pulse_64Point ); 00074 00075 Pulse_96Point += Buff_1Point[CountV255]; 00076 Pulse_96Point -= Buff_1Point[(CountV255-96) & 0xff]; // заполнение буфера накопленых приращений за 96 тактов 00077 Buff_96Point[CountV31] = (Pulse_96Point ); 00078 00079 Pulse_128Point += Buff_1Point[CountV255]; 00080 Pulse_128Point -= Buff_1Point[(CountV255-128) & 0xff]; // заполнение буфера накопленых приращений за 128 тактов 00081 Buff_128Point[CountV31] = (Pulse_128Point ); 00082 00083 00084 Pulse_16PointD += Buff_1Point[CountV255]; 00085 Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз 00086 Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; // 00087 Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // 00088 Buff_16PointD[CountV31] = Pulse_16PointD ; 00089 00090 00091 00092 00093 00094 00095 00096 00097 00098 00099 00100 Buff_Restored_sin [CountV31] = (Buff_16Point[CountV31])*2 - Buff_32Point[CountV31]; 00101 00102 if((Buff_Restored_sin [CountV31]) > 0) CaunAddPlus += Buff_Restored_sin [CountV31]; // счетчик положительных импульсов 00103 else CaunAddMin -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды 00104 00105 00106 // if((Buff_1Point[CountV31]) > 0) Gyro.CaunPlusRateAdd += Buff_1Point[CountV255]; // счетчик положительных импульсов 00107 // else Gyro.CaunMinRateAdd -= Buff_1Point[CountV255]; // счетчик отрицательных импульсов // расчет амплитуды 00108 /* 00109 if(Buff_Restored_sin [CountV31] > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик положительных импульсов 00110 else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик отрицательных импульсов // расчет амплитуды 00111 */ 00112 00113 // if(Buff_Restored_sin [CountV31]>0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255 - 8 /*Gyro.FrqPhase*/)&0xff]; // счетчик положительных импульсов 00114 // else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255 - 8 /* Gyro.FrqPhase*/)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды 00115 00116 //if((Buff_1Point[CountV255]) > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255)&0xff]; // счетчик положительных импульсов 00117 //else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды*/ 00118 00119 00120 // if (LPC_QEI->STAT) Gyro.CaunMinReperAdd -= Dif_QEI; 00121 // else Gyro.CaunPlusReperAdd += Dif_QEI; 00122 00123 00124 00125 00126 00127 00128 switch(CountV31){ 00129 case 31: 00130 Gyro.CaunMinRate = Gyro.CaunMinRateAdd; 00131 Gyro.CaunPlusRate = Gyro.CaunPlusRateAdd; 00132 Gyro.CaunMinRateAdd = 0; 00133 Gyro.CaunPlusRateAdd = 0; 00134 break; 00135 /* case 10: 00136 00137 00138 sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper); 00139 WriteCon(Time); */ 00140 00141 break; 00142 } 00143 if(Gyro.FrqPhase==CountV31) 00144 { 00145 //tempReper2=0; 00146 /* Gyro.CaunMinReper = Gyro.CaunMinReperAdd; 00147 Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; 00148 Gyro.CaunMinReperAdd = 0; 00149 Gyro.CaunPlusReperAdd = 0;*/ 00150 } 00151 00152 00153 00154 if(Dif_QEI > 0) 00155 { 00156 if(tempReper) 00157 { 00158 Gyro.CaunMinReper = Gyro.CaunMinReperAdd; 00159 Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; 00160 Gyro.CaunMinReperAdd = 0; 00161 Gyro.CaunPlusReperAdd = 0; 00162 Gyro.Reper_Event=1; 00163 // Gyro.CuruAngle += Gyro.CaunPlusReper-Gyro.CaunMinReper; 00164 tempReper=0; 00165 } 00166 Gyro.CaunPlusReperAdd += Dif_QEI; 00167 } 00168 else 00169 { 00170 tempReper=1; 00171 Gyro.CaunMinReperAdd -= Dif_QEI; 00172 00173 } 00174 00175 Gyro.CuruAngle += Buff_32Point [CountV31]; 00176 Gyro.CuruAngle64 += Buff_64Point [CountV31]; 00177 Gyro.CuruAngle96 += Buff_96Point [CountV31]; 00178 Gyro.CuruAngle128 += Buff_128Point [CountV31]; 00179 00180 00181 00182 } 00183 00184
Generated on Thu Jul 14 2022 02:34:42 by
1.7.2