Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

QEI.c@231:079835d508ef, 2018-01-25 (annotated)

Committer:
Kovalev_D
Date:
Thu Jan 25 13:59:43 2018 +0000
Revision:
231:079835d508ef
Parent:
214:4c70e452c491
Child:
232:130a2b5003e6
termo

Who changed what in which revision?

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