Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

QEI.c@219:2d3475d0dd1b, 2017-09-27 (annotated)

Committer:
Kovalev_D
Date:
Wed Sep 27 13:09:24 2017 +0000
Revision:
219:2d3475d0dd1b
Parent:
214:4c70e452c491 
hnjtfgyy

Who changed what in which revision?

UserRevisionLine numberNew 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 211:ac8251b067d2 8 unsigned int tempReper,tempReper2;
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 211:ac8251b067d2 13 int Pulse_96Point;
Kovalev_D 211:ac8251b067d2 14 int Pulse_128Point;
Kovalev_D 86:398da56ef751 15 int Pulse_16PointD;
Kovalev_D 191:40028201ddad 16 unsigned int CaunAddPlus =0;//счетчик ипульсов энкодера"+" за такт ВП
Kovalev_D 192:d32c8cf7bcd9 17 unsigned int CaunAddMin =0;//счетчик ипульсов энкодера"-" за такт ВП
Kovalev_D 108:030cdde08314 18
Kovalev_D 112:4a96133a1311 19 int unsigned Cur_QEI, Last_QEI; //текушее и предыдущее,(единичное) значение энкодера
Kovalev_D 207:d1ce992f5d17 20 int temp32=0;
Kovalev_D 193:a0fe8bfc97e4 21 int Buff_1Point [512];
Kovalev_D 183:7e200f4d9b16 22 int Buff_32Point [32];
Kovalev_D 211:ac8251b067d2 23 int Buff_64Point [32];
Kovalev_D 211:ac8251b067d2 24 int Buff_96Point [32];
Kovalev_D 211:ac8251b067d2 25 int Buff_128Point [32];
igor_v 43:3f7c8a0ea59c 26 int Buff_16Point [32];
igor_v 43:3f7c8a0ea59c 27 int Buff_16PointD [32];
igor_v 43:3f7c8a0ea59c 28 int Buff_8Point [32];
igor_v 43:3f7c8a0ea59c 29 int Buff_Restored_sin [32];
Kovalev_D 196:f76dbc081e63 30 int Buff_Restored_sin2 [32];
igor_v 21:bc8c1cec3da6 31 void D_QEI(void)
igor_v 21:bc8c1cec3da6 32 {
Kovalev_D 209:224e7331a061 33
igor_v 21:bc8c1cec3da6 34 Dif_QEI=0;
igor_v 21:bc8c1cec3da6 35 Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
Kovalev_D 207:d1ce992f5d17 36
Kovalev_D 207:d1ce992f5d17 37
Kovalev_D 207:d1ce992f5d17 38
Kovalev_D 197:7a05523bf588 39 //LPC_QEI->CON = 0xF;
igor_v 21:bc8c1cec3da6 40 Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.()
Kovalev_D 208:19150d2b528f 41 /* temp32 = Dif_QEI;
Kovalev_D 207:d1ce992f5d17 42 Gyro.CuruAngle32 += temp32;
Kovalev_D 208:19150d2b528f 43 temp32=0;*/
igor_v 21:bc8c1cec3da6 44 Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения.
Kovalev_D 207:d1ce992f5d17 45
Kovalev_D 211:ac8251b067d2 46
Kovalev_D 211:ac8251b067d2 47
igor_v 21:bc8c1cec3da6 48 if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль
igor_v 21:bc8c1cec3da6 49 if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль
Kovalev_D 211:ac8251b067d2 50
Kovalev_D 211:ac8251b067d2 51 /* sprintf((Time)," %d %d %d \r\n",Gyro.CaunPlusReperAdd,Gyro.CaunMinReperAdd, Dif_QEI);
Kovalev_D 211:ac8251b067d2 52 WriteCon(Time);*/
Kovalev_D 108:030cdde08314 53 Buff_1Point[CountV255] = Dif_QEI ; // накопление в буфер еденичных значений приращения по каждому такту.
Kovalev_D 208:19150d2b528f 54
Kovalev_D 208:19150d2b528f 55
Kovalev_D 205:775d54fdf646 56
igor_v 21:bc8c1cec3da6 57 //////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 108:030cdde08314 58 Pulse_8Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 59 Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов
Kovalev_D 112:4a96133a1311 60 Buff_8Point[CountV31] = (Pulse_8Point);
igor_v 0:8ad47e2b6f00 61
igor_v 21:bc8c1cec3da6 62 Pulse_16Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 63 Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов
Kovalev_D 112:4a96133a1311 64 Buff_16Point[CountV31] = (Pulse_16Point );
igor_v 21:bc8c1cec3da6 65
igor_v 21:bc8c1cec3da6 66 Pulse_32Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 67 Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов
Kovalev_D 112:4a96133a1311 68 Gyro.Cnt_Dif = (Pulse_32Point);
Kovalev_D 112:4a96133a1311 69 Buff_32Point[CountV31] = (Pulse_32Point );
Kovalev_D 209:224e7331a061 70
Kovalev_D 193:a0fe8bfc97e4 71 Pulse_64Point += Buff_1Point[CountV255];
Kovalev_D 211:ac8251b067d2 72 Pulse_64Point -= Buff_1Point[(CountV255-64) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов
Kovalev_D 183:7e200f4d9b16 73 Buff_64Point[CountV31] = (Pulse_64Point );
Kovalev_D 211:ac8251b067d2 74
Kovalev_D 211:ac8251b067d2 75 Pulse_96Point += Buff_1Point[CountV255];
Kovalev_D 211:ac8251b067d2 76 Pulse_96Point -= Buff_1Point[(CountV255-96) & 0xff]; // заполнение буфера накопленых приращений за 96 тактов
Kovalev_D 211:ac8251b067d2 77 Buff_96Point[CountV31] = (Pulse_96Point );
Kovalev_D 211:ac8251b067d2 78
Kovalev_D 211:ac8251b067d2 79 Pulse_128Point += Buff_1Point[CountV255];
Kovalev_D 211:ac8251b067d2 80 Pulse_128Point -= Buff_1Point[(CountV255-128) & 0xff]; // заполнение буфера накопленых приращений за 128 тактов
Kovalev_D 211:ac8251b067d2 81 Buff_128Point[CountV31] = (Pulse_128Point );
Kovalev_D 193:a0fe8bfc97e4 82
igor_v 0:8ad47e2b6f00 83
igor_v 21:bc8c1cec3da6 84 Pulse_16PointD += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 85 Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз
igor_v 21:bc8c1cec3da6 86 Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; //
Kovalev_D 169:140743e3bb96 87 Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; //
Kovalev_D 108:030cdde08314 88 Buff_16PointD[CountV31] = Pulse_16PointD ;
Kovalev_D 183:7e200f4d9b16 89
Kovalev_D 214:4c70e452c491 90
Kovalev_D 214:4c70e452c491 91
Kovalev_D 214:4c70e452c491 92
Kovalev_D 214:4c70e452c491 93
Kovalev_D 214:4c70e452c491 94
Kovalev_D 214:4c70e452c491 95
Kovalev_D 214:4c70e452c491 96
Kovalev_D 214:4c70e452c491 97
Kovalev_D 214:4c70e452c491 98
Kovalev_D 214:4c70e452c491 99
Kovalev_D 211:ac8251b067d2 100 Buff_Restored_sin [CountV31] = (Buff_16Point[CountV31])*2 - Buff_32Point[CountV31];
Kovalev_D 209:224e7331a061 101
Kovalev_D 214:4c70e452c491 102 if((Buff_Restored_sin [CountV31]) > 0) CaunAddPlus += Buff_Restored_sin [CountV31]; // счетчик положительных импульсов
Kovalev_D 209:224e7331a061 103 else CaunAddMin -= Buff_Restored_sin [CountV31]; //счетчик отрицательных импульсов // расчет амплитуды
Kovalev_D 209:224e7331a061 104
Kovalev_D 209:224e7331a061 105
Kovalev_D 211:ac8251b067d2 106 // if((Buff_1Point[CountV31]) > 0) Gyro.CaunPlusRateAdd += Buff_1Point[CountV255]; // счетчик положительных импульсов
Kovalev_D 211:ac8251b067d2 107 // else Gyro.CaunMinRateAdd -= Buff_1Point[CountV255]; // счетчик отрицательных импульсов // расчет амплитуды
Kovalev_D 209:224e7331a061 108 /*
Kovalev_D 209:224e7331a061 109 if(Buff_Restored_sin [CountV31] > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик положительных импульсов
Kovalev_D 209:224e7331a061 110 else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV31-Gyro.FrqPhase)&0x1f]; // счетчик отрицательных импульсов // расчет амплитуды
Kovalev_D 211:ac8251b067d2 111 */
Kovalev_D 211:ac8251b067d2 112
Kovalev_D 211:ac8251b067d2 113 // if(Buff_Restored_sin [CountV31]>0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255 - 8 /*Gyro.FrqPhase*/)&0xff]; // счетчик положительных импульсов
Kovalev_D 211:ac8251b067d2 114 // else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255 - 8 /* Gyro.FrqPhase*/)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды
Kovalev_D 211:ac8251b067d2 115
Kovalev_D 211:ac8251b067d2 116 //if((Buff_1Point[CountV255]) > 0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255)&0xff]; // счетчик положительных импульсов
Kovalev_D 211:ac8251b067d2 117 //else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255)&0xff]; // счетчик отрицательных импульсов // расчет амплитуды*/
Kovalev_D 211:ac8251b067d2 118
Kovalev_D 211:ac8251b067d2 119
Kovalev_D 211:ac8251b067d2 120 // if (LPC_QEI->STAT) Gyro.CaunMinReperAdd -= Dif_QEI;
Kovalev_D 211:ac8251b067d2 121 // else Gyro.CaunPlusReperAdd += Dif_QEI;
Kovalev_D 211:ac8251b067d2 122
Kovalev_D 211:ac8251b067d2 123
Kovalev_D 211:ac8251b067d2 124
Kovalev_D 211:ac8251b067d2 125
Kovalev_D 211:ac8251b067d2 126
Kovalev_D 209:224e7331a061 127
Kovalev_D 209:224e7331a061 128 switch(CountV31){
Kovalev_D 209:224e7331a061 129 case 31:
Kovalev_D 209:224e7331a061 130 Gyro.CaunMinRate = Gyro.CaunMinRateAdd;
Kovalev_D 209:224e7331a061 131 Gyro.CaunPlusRate = Gyro.CaunPlusRateAdd;
Kovalev_D 209:224e7331a061 132 Gyro.CaunMinRateAdd = 0;
Kovalev_D 209:224e7331a061 133 Gyro.CaunPlusRateAdd = 0;
Kovalev_D 209:224e7331a061 134 break;
Kovalev_D 210:b02fa166315d 135 /* case 10:
Kovalev_D 210:b02fa166315d 136
Kovalev_D 211:ac8251b067d2 137
Kovalev_D 211:ac8251b067d2 138 sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper);
Kovalev_D 211:ac8251b067d2 139 WriteCon(Time); */
Kovalev_D 210:b02fa166315d 140
Kovalev_D 209:224e7331a061 141 break;
Kovalev_D 209:224e7331a061 142 }
Kovalev_D 210:b02fa166315d 143 if(Gyro.FrqPhase==CountV31)
Kovalev_D 210:b02fa166315d 144 {
Kovalev_D 211:ac8251b067d2 145 //tempReper2=0;
Kovalev_D 211:ac8251b067d2 146 /* Gyro.CaunMinReper = Gyro.CaunMinReperAdd;
Kovalev_D 210:b02fa166315d 147 Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
Kovalev_D 210:b02fa166315d 148 Gyro.CaunMinReperAdd = 0;
Kovalev_D 211:ac8251b067d2 149 Gyro.CaunPlusReperAdd = 0;*/
Kovalev_D 210:b02fa166315d 150 }
Kovalev_D 211:ac8251b067d2 151
Kovalev_D 211:ac8251b067d2 152
Kovalev_D 211:ac8251b067d2 153
Kovalev_D 211:ac8251b067d2 154 if(Dif_QEI > 0)
Kovalev_D 211:ac8251b067d2 155 {
Kovalev_D 211:ac8251b067d2 156 if(tempReper)
Kovalev_D 211:ac8251b067d2 157 {
Kovalev_D 211:ac8251b067d2 158 Gyro.CaunMinReper = Gyro.CaunMinReperAdd;
Kovalev_D 211:ac8251b067d2 159 Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
Kovalev_D 211:ac8251b067d2 160 Gyro.CaunMinReperAdd = 0;
Kovalev_D 211:ac8251b067d2 161 Gyro.CaunPlusReperAdd = 0;
Kovalev_D 211:ac8251b067d2 162 Gyro.Reper_Event=1;
Kovalev_D 211:ac8251b067d2 163 // Gyro.CuruAngle += Gyro.CaunPlusReper-Gyro.CaunMinReper;
Kovalev_D 211:ac8251b067d2 164 tempReper=0;
Kovalev_D 211:ac8251b067d2 165 }
Kovalev_D 211:ac8251b067d2 166 Gyro.CaunPlusReperAdd += Dif_QEI;
Kovalev_D 211:ac8251b067d2 167 }
Kovalev_D 211:ac8251b067d2 168 else
Kovalev_D 211:ac8251b067d2 169 {
Kovalev_D 211:ac8251b067d2 170 tempReper=1;
Kovalev_D 211:ac8251b067d2 171 Gyro.CaunMinReperAdd -= Dif_QEI;
Kovalev_D 211:ac8251b067d2 172
Kovalev_D 211:ac8251b067d2 173 }
Kovalev_D 211:ac8251b067d2 174
Kovalev_D 211:ac8251b067d2 175 Gyro.CuruAngle += Buff_32Point [CountV31];
Kovalev_D 211:ac8251b067d2 176 Gyro.CuruAngle64 += Buff_64Point [CountV31];
Kovalev_D 211:ac8251b067d2 177 Gyro.CuruAngle96 += Buff_96Point [CountV31];
Kovalev_D 211:ac8251b067d2 178 Gyro.CuruAngle128 += Buff_128Point [CountV31];
Kovalev_D 211:ac8251b067d2 179
Kovalev_D 211:ac8251b067d2 180
Kovalev_D 210:b02fa166315d 181
igor_v 21:bc8c1cec3da6 182 }
Kovalev_D 211:ac8251b067d2 183
Kovalev_D 193:a0fe8bfc97e4 184