Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

QEI.c@209:224e7331a061, 2017-04-13 (annotated)

Committer:
Kovalev_D
Date:
Thu Apr 13 14:14:45 2017 +0000
Revision:
209:224e7331a061
Parent:
208:19150d2b528f
Child:
210:b02fa166315d
v2

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 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 209:224e7331a061 86 if(Buff_Restored_sin [CountV31]>0) Gyro.CaunPlusReperAdd += Buff_1Point[(CountV255-7)&0xff]; // счетчик положительных импульсов
Kovalev_D 209:224e7331a061 87 else Gyro.CaunMinReperAdd -= Buff_1Point[(CountV255-7)&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 209:224e7331a061 99 case 10:
Kovalev_D 209:224e7331a061 100 Gyro.CaunMinReper = Gyro.CaunMinReperAdd;
Kovalev_D 209:224e7331a061 101 Gyro.CaunPlusReper = Gyro.CaunPlusReperAdd;
Kovalev_D 209:224e7331a061 102 Gyro.CaunMinReperAdd = 0;
Kovalev_D 209:224e7331a061 103 Gyro.CaunPlusReperAdd = 0;
Kovalev_D 209:224e7331a061 104
Kovalev_D 209:224e7331a061 105 /* sprintf((Time)," %d %d \r\n", Gyro.CaunMinReper, Gyro.CaunPlusReper);
Kovalev_D 209:224e7331a061 106 WriteCon(Time); */
Kovalev_D 209:224e7331a061 107 break;
Kovalev_D 209:224e7331a061 108 }
Kovalev_D 209:224e7331a061 109
Kovalev_D 209:224e7331a061 110 if(Buff_Restored_sin [CountV31] > 0)
Kovalev_D 209:224e7331a061 111 {
Kovalev_D 209:224e7331a061 112 if(Gyro.Reper_Event)tempReper=0;
Kovalev_D 209:224e7331a061 113 else tempReper=1;
Kovalev_D 209:224e7331a061 114 }
Kovalev_D 209:224e7331a061 115 else
Kovalev_D 209:224e7331a061 116 {
Kovalev_D 209:224e7331a061 117 if(tempReper)
Kovalev_D 209:224e7331a061 118 {
Kovalev_D 209:224e7331a061 119 tempReper=0;
Kovalev_D 209:224e7331a061 120 Gyro.Reper_Event=1;
Kovalev_D 209:224e7331a061 121 }
Kovalev_D 209:224e7331a061 122 }
Kovalev_D 209:224e7331a061 123
Kovalev_D 197:7a05523bf588 124 Gyro.CuruAngle += Buff_32Point [CountV31];
Kovalev_D 208:19150d2b528f 125 //LoopOff
igor_v 21:bc8c1cec3da6 126 }
Kovalev_D 196:f76dbc081e63 127 /*
Kovalev_D 193:a0fe8bfc97e4 128 void CurAngle(void)
Kovalev_D 193:a0fe8bfc97e4 129 {
Kovalev_D 193:a0fe8bfc97e4 130 for (int i = 0; i < 32; i++ )
Kovalev_D 193:a0fe8bfc97e4 131 {
Kovalev_D 197:7a05523bf588 132 Gyro.CuruAngle += Buff_32Point[i];
Kovalev_D 193:a0fe8bfc97e4 133 }
Kovalev_D 193:a0fe8bfc97e4 134 }
Kovalev_D 193:a0fe8bfc97e4 135 void CurAngleLOG(void)
Kovalev_D 193:a0fe8bfc97e4 136 {
Kovalev_D 193:a0fe8bfc97e4 137 sprintf((Time)," %d %d \r\n", Gyro.tempdelta,Gyro.CuruAngle);
Kovalev_D 193:a0fe8bfc97e4 138 WriteCon(Time);
Kovalev_D 193:a0fe8bfc97e4 139 Gyro.CuruAngle=0;
Kovalev_D 193:a0fe8bfc97e4 140 Gyro.tempdelta=0;
Kovalev_D 196:f76dbc081e63 141 }*/
Kovalev_D 193:a0fe8bfc97e4 142