Dmitry Kovalev
/
LG2
fork
Fork of LG by
QEI.c@210:b02fa166315d, 2017-04-24 (annotated)
- Committer:
- Kovalev_D
- Date:
- Mon Apr 24 05:43:54 2017 +0000
- Revision:
- 210:b02fa166315d
- Parent:
- 209:224e7331a061
- Child:
- 211:ac8251b067d2
123
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 1:f2adcae3d304 | 1 | #include "Global.h" |
igor_v | 0:8ad47e2b6f00 | 2 | |
igor_v | 21:bc8c1cec3da6 | 3 | int Dif_QEI; |
igor_v | 42:6fc307c4963e | 4 | |
igor_v | 21:bc8c1cec3da6 | 5 | int FFF=0;//для заплатки |
igor_v | 0:8ad47e2b6f00 | 6 | |
Kovalev_D | 197:7a05523bf588 | 7 | unsigned int tempmod=64,tempmod2=1; |
Kovalev_D | 209:224e7331a061 | 8 | unsigned int tempReper; |
Kovalev_D | 86:398da56ef751 | 9 | int Pulse_8Point; |
Kovalev_D | 86:398da56ef751 | 10 | int Pulse_16Point; |
Kovalev_D | 86:398da56ef751 | 11 | int Pulse_32Point; |
Kovalev_D | 183:7e200f4d9b16 | 12 | int Pulse_64Point; |
Kovalev_D | 86:398da56ef751 | 13 | int Pulse_16PointD; |
Kovalev_D | 191:40028201ddad | 14 | unsigned int CaunAddPlus =0;//счетчик ипульсов энкодера"+" за такт ВП |
Kovalev_D | 192:d32c8cf7bcd9 | 15 | unsigned int CaunAddMin =0;//счетчик ипульсов энкодера"-" за такт ВП |
Kovalev_D | 108:030cdde08314 | 16 | |
Kovalev_D | 112:4a96133a1311 | 17 | int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера |
Kovalev_D | 207:d1ce992f5d17 | 18 | int temp32=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 19 | int Buff_1Point [512]; |
Kovalev_D | 183:7e200f4d9b16 | 20 | int Buff_32Point [32]; |
Kovalev_D | 193:a0fe8bfc97e4 | 21 | int Buff_64Point [32]; |
igor_v | 43:3f7c8a0ea59c | 22 | int Buff_16Point [32]; |
igor_v | 43:3f7c8a0ea59c | 23 | int Buff_16PointD [32]; |
igor_v | 43:3f7c8a0ea59c | 24 | int Buff_8Point [32]; |
igor_v | 43:3f7c8a0ea59c | 25 | int Buff_Restored_sin [32]; |
Kovalev_D | 196:f76dbc081e63 | 26 | int Buff_Restored_sin2 [32]; |
igor_v | 21:bc8c1cec3da6 | 27 | void D_QEI(void) |
igor_v | 21:bc8c1cec3da6 | 28 | { |
Kovalev_D | 209:224e7331a061 | 29 | |
igor_v | 21:bc8c1cec3da6 | 30 | Dif_QEI=0; |
igor_v | 21:bc8c1cec3da6 | 31 | Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера. |
Kovalev_D | 207:d1ce992f5d17 | 32 | |
Kovalev_D | 207:d1ce992f5d17 | 33 | |
Kovalev_D | 207:d1ce992f5d17 | 34 | |
Kovalev_D | 197:7a05523bf588 | 35 | //LPC_QEI->CON = 0xF; |
igor_v | 21:bc8c1cec3da6 | 36 | Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.() |
Kovalev_D | 208:19150d2b528f | 37 | /* temp32 = Dif_QEI; |
Kovalev_D | 207:d1ce992f5d17 | 38 | Gyro.CuruAngle32 += temp32; |
Kovalev_D | 208:19150d2b528f | 39 | temp32=0;*/ |
igor_v | 21:bc8c1cec3da6 | 40 | Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения. |
Kovalev_D | 207:d1ce992f5d17 | 41 | |
igor_v | 21:bc8c1cec3da6 | 42 | if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль |
igor_v | 21:bc8c1cec3da6 | 43 | if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль |
Kovalev_D | 108:030cdde08314 | 44 | Buff_1Point[CountV255] = Dif_QEI ; // накопление в буфер еденичных значений приращения по каждому такту. |
Kovalev_D | 208:19150d2b528f | 45 | |
Kovalev_D | 208:19150d2b528f | 46 | |
Kovalev_D | 205:775d54fdf646 | 47 | |
igor_v | 21:bc8c1cec3da6 | 48 | ////////////////////////////////////////////////////////////////////////////////////////////// |
Kovalev_D | 108:030cdde08314 | 49 | Pulse_8Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 50 | Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов |
Kovalev_D | 112:4a96133a1311 | 51 | Buff_8Point[CountV31] = (Pulse_8Point); |
igor_v | 0:8ad47e2b6f00 | 52 | |
igor_v | 21:bc8c1cec3da6 | 53 | Pulse_16Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 54 | Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов |
Kovalev_D | 112:4a96133a1311 | 55 | Buff_16Point[CountV31] = (Pulse_16Point ); |
igor_v | 21:bc8c1cec3da6 | 56 | |
igor_v | 21:bc8c1cec3da6 | 57 | Pulse_32Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 58 | Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов |
Kovalev_D | 112:4a96133a1311 | 59 | Gyro.Cnt_Dif = (Pulse_32Point); |
Kovalev_D | 112:4a96133a1311 | 60 | Buff_32Point[CountV31] = (Pulse_32Point ); |
Kovalev_D | 209:224e7331a061 | 61 | |
Kovalev_D | 193:a0fe8bfc97e4 | 62 | Pulse_64Point += Buff_1Point[CountV255]; |
Kovalev_D | 208:19150d2b528f | 63 | Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за два 32 тактов |
Kovalev_D | 208:19150d2b528f | 64 | // Gyro.Cnt_Dif = (Pulse_64Point); |
Kovalev_D | 183:7e200f4d9b16 | 65 | Buff_64Point[CountV31] = (Pulse_64Point ); |
Kovalev_D | 193:a0fe8bfc97e4 | 66 | |
igor_v | 0:8ad47e2b6f00 | 67 | |
igor_v | 21:bc8c1cec3da6 | 68 | Pulse_16PointD += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 69 | Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз |
igor_v | 21:bc8c1cec3da6 | 70 | Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; // |
Kovalev_D | 169:140743e3bb96 | 71 | Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // |
Kovalev_D | 108:030cdde08314 | 72 | Buff_16PointD[CountV31] = Pulse_16PointD ; |
Kovalev_D | 183:7e200f4d9b16 | 73 | |
Kovalev_D | 196:f76dbc081e63 | 74 | Buff_Restored_sin [CountV31] = (Buff_16PointD[CountV31]) - Buff_32Point[CountV31]; |
Kovalev_D | 209:224e7331a061 | 75 | |
Kovalev_D | 209:224e7331a061 | 76 | if((Buff_Restored_sin [CountV31]) > 0) CaunAddPlus += Buff_Restored_sin [CountV31]; // счетчик положительных импульсов |
Kovalev_D | 209:224e7331a061 | 77 | else CaunAddMin -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды |
Kovalev_D | 209:224e7331a061 | 78 | |
Kovalev_D | 209:224e7331a061 | 79 | |
Kovalev_D | 209:224e7331a061 | 80 | if((Buff_1Point[CountV31]) > 0) Gyro.CaunPlusRateAdd += Buff_1Point[CountV255]; // счетчик положительных импульсов |
Kovalev_D | 209:224e7331a061 | 81 | else Gyro.CaunMinRateAdd -= Buff_1Point[CountV255]; // счетчик отрицательных импульсов // расчет амплитуды |
Kovalev_D | 209:224e7331a061 | 82 | /* |
Kovalev_D | 209:224e7331a061 | 83 | if(Buff_Restored_sin [CountV31] > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик положительных импульсов |
Kovalev_D | 209:224e7331a061 | 84 | else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик отрицательных импульсов // расчет амплитуды |
Kovalev_D | 209:224e7331a061 | 85 | **/ |
Kovalev_D | 210:b02fa166315d | 86 | if(Buff_Restored_sin [CountV31]>0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255 - 8/*Gyro.FrqPhase */)&0xff]; // счетчик положительных импульсов |
Kovalev_D | 210:b02fa166315d | 87 | else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255 - 8/*Gyro.FrqPhase*/ )&0xff]; // счетчик отрицательных импульсов // расчет амплитуды |
Kovalev_D | 209:224e7331a061 | 88 | |
Kovalev_D | 209:224e7331a061 | 89 | /*if((Buff_1Point[CountV31]) > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255-7)&0xff]; // счетчик положительных импульсов |
Kovalev_D | 209:224e7331a061 | 90 | else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255-7)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды*/ |
Kovalev_D | 209:224e7331a061 | 91 | |
Kovalev_D | 209:224e7331a061 | 92 | switch(CountV31){ |
Kovalev_D | 209:224e7331a061 | 93 | case 31: |
Kovalev_D | 209:224e7331a061 | 94 | Gyro.CaunMinRate = Gyro.CaunMinRateAdd; |
Kovalev_D | 209:224e7331a061 | 95 | Gyro.CaunPlusRate = Gyro.CaunPlusRateAdd; |
Kovalev_D | 209:224e7331a061 | 96 | Gyro.CaunMinRateAdd = 0; |
Kovalev_D | 209:224e7331a061 | 97 | Gyro.CaunPlusRateAdd = 0; |
Kovalev_D | 209:224e7331a061 | 98 | break; |
Kovalev_D | 210:b02fa166315d | 99 | /* case 10: |
Kovalev_D | 210:b02fa166315d | 100 | |
Kovalev_D | 210:b02fa166315d | 101 | Gyro.CaunMinReper = Gyro.CaunMinReperAdd; |
Kovalev_D | 210:b02fa166315d | 102 | Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; |
Kovalev_D | 210:b02fa166315d | 103 | Gyro.CaunMinReperAdd = 0; |
Kovalev_D | 210:b02fa166315d | 104 | Gyro.CaunPlusReperAdd = 0; |
Kovalev_D | 210:b02fa166315d | 105 | sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper); |
Kovalev_D | 210:b02fa166315d | 106 | WriteCon(Time); */ |
Kovalev_D | 210:b02fa166315d | 107 | |
Kovalev_D | 209:224e7331a061 | 108 | break; |
Kovalev_D | 209:224e7331a061 | 109 | } |
Kovalev_D | 210:b02fa166315d | 110 | if(Gyro.FrqPhase==CountV31) |
Kovalev_D | 210:b02fa166315d | 111 | { |
Kovalev_D | 210:b02fa166315d | 112 | Gyro.CaunMinReper = Gyro.CaunMinReperAdd; |
Kovalev_D | 210:b02fa166315d | 113 | Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd; |
Kovalev_D | 210:b02fa166315d | 114 | Gyro.CaunMinReperAdd = 0; |
Kovalev_D | 210:b02fa166315d | 115 | Gyro.CaunPlusReperAdd = 0; |
Kovalev_D | 210:b02fa166315d | 116 | } |
Kovalev_D | 209:224e7331a061 | 117 | if(Buff_Restored_sin [CountV31] > 0) |
Kovalev_D | 209:224e7331a061 | 118 | { |
Kovalev_D | 209:224e7331a061 | 119 | if(Gyro.Reper_Event)tempReper=0; |
Kovalev_D | 209:224e7331a061 | 120 | else tempReper=1; |
Kovalev_D | 209:224e7331a061 | 121 | } |
Kovalev_D | 209:224e7331a061 | 122 | else |
Kovalev_D | 209:224e7331a061 | 123 | { |
Kovalev_D | 209:224e7331a061 | 124 | if(tempReper) |
Kovalev_D | 209:224e7331a061 | 125 | { |
Kovalev_D | 210:b02fa166315d | 126 | |
Kovalev_D | 210:b02fa166315d | 127 | |
Kovalev_D | 210:b02fa166315d | 128 | |
Kovalev_D | 209:224e7331a061 | 129 | tempReper=0; |
Kovalev_D | 209:224e7331a061 | 130 | Gyro.Reper_Event=1; |
Kovalev_D | 210:b02fa166315d | 131 | |
Kovalev_D | 210:b02fa166315d | 132 | |
Kovalev_D | 210:b02fa166315d | 133 | |
Kovalev_D | 209:224e7331a061 | 134 | } |
Kovalev_D | 209:224e7331a061 | 135 | } |
Kovalev_D | 209:224e7331a061 | 136 | |
Kovalev_D | 197:7a05523bf588 | 137 | Gyro.CuruAngle += Buff_32Point [CountV31]; |
Kovalev_D | 208:19150d2b528f | 138 | //LoopOff |
igor_v | 21:bc8c1cec3da6 | 139 | } |
Kovalev_D | 196:f76dbc081e63 | 140 | /* |
Kovalev_D | 193:a0fe8bfc97e4 | 141 | void CurAngle(void) |
Kovalev_D | 193:a0fe8bfc97e4 | 142 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 143 | for (int i = 0; i < 32; i++ ) |
Kovalev_D | 193:a0fe8bfc97e4 | 144 | { |
Kovalev_D | 197:7a05523bf588 | 145 | Gyro.CuruAngle += Buff_32Point[i]; |
Kovalev_D | 193:a0fe8bfc97e4 | 146 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 147 | } |
Kovalev_D | 193:a0fe8bfc97e4 | 148 | void CurAngleLOG(void) |
Kovalev_D | 193:a0fe8bfc97e4 | 149 | { |
Kovalev_D | 193:a0fe8bfc97e4 | 150 | sprintf((Time)," %d %d \r\n", Gyro.tempdelta,Gyro.CuruAngle); |
Kovalev_D | 193:a0fe8bfc97e4 | 151 | WriteCon(Time); |
Kovalev_D | 193:a0fe8bfc97e4 | 152 | Gyro.CuruAngle=0; |
Kovalev_D | 193:a0fe8bfc97e4 | 153 | Gyro.tempdelta=0; |
Kovalev_D | 196:f76dbc081e63 | 154 | }*/ |
Kovalev_D | 193:a0fe8bfc97e4 | 155 |