Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

QEI.c@86:398da56ef751, 2016-03-24 (annotated)

Committer:
Kovalev_D
Date:
Thu Mar 24 08:20:43 2016 +0000
Revision:
86:398da56ef751
Parent:
45:cbc955aecebe
Child:
87:7e575d26d6d0
GLD_Init

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
Kovalev_D 86:398da56ef751 4
Kovalev_D 86:398da56ef751 5
Kovalev_D 86:398da56ef751 6 int CuruAngle ;
Kovalev_D 86:398da56ef751 7
igor_v 21:bc8c1cec3da6 8 int Dif_QEI;
igor_v 42:6fc307c4963e 9
igor_v 21:bc8c1cec3da6 10 int FFF=0;//для заплатки
igor_v 0:8ad47e2b6f00 11
igor_v 42:6fc307c4963e 12
Kovalev_D 86:398da56ef751 13 int Pulse_8Point;
Kovalev_D 86:398da56ef751 14 int Pulse_16Point;
Kovalev_D 86:398da56ef751 15 int Pulse_32Point;
Kovalev_D 86:398da56ef751 16 int Pulse_16PointD;
igor_v 21:bc8c1cec3da6 17 unsigned int Iras=0,Temp_F_ras=0;
igor_v 21:bc8c1cec3da6 18 int yy = 0;
igor_v 0:8ad47e2b6f00 19
igor_v 0:8ad47e2b6f00 20
igor_v 42:6fc307c4963e 21
igor_v 42:6fc307c4963e 22
Kovalev_D 86:398da56ef751 23 int Buff_32Point [32];
igor_v 43:3f7c8a0ea59c 24 int Buff_16Point [32];
igor_v 43:3f7c8a0ea59c 25 int Buff_16PointD [32];
igor_v 43:3f7c8a0ea59c 26 int Buff_8Point [32];
igor_v 43:3f7c8a0ea59c 27 int Buff_Restored_sin [32];
igor_v 43:3f7c8a0ea59c 28 int Buff_1Point [256]; // этот на 256
igor_v 42:6fc307c4963e 29
igor_v 42:6fc307c4963e 30
igor_v 42:6fc307c4963e 31
Kovalev_D 86:398da56ef751 32 int unsigned Cur_QEI, Last_QEI;
igor_v 21:bc8c1cec3da6 33
igor_v 21:bc8c1cec3da6 34
igor_v 42:6fc307c4963e 35
igor_v 42:6fc307c4963e 36
igor_v 42:6fc307c4963e 37
igor_v 42:6fc307c4963e 38
igor_v 42:6fc307c4963e 39
igor_v 43:3f7c8a0ea59c 40 // ДАВАЙ попробуем сделать буфера в целых (знаковы)
Kovalev_D 86:398da56ef751 41
Kovalev_D 86:398da56ef751 42
igor_v 42:6fc307c4963e 43 void D_QEI_Int(void)
igor_v 42:6fc307c4963e 44 {
igor_v 42:6fc307c4963e 45
igor_v 43:3f7c8a0ea59c 46 Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
igor_v 42:6fc307c4963e 47 Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.()
igor_v 42:6fc307c4963e 48 Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения.
igor_v 42:6fc307c4963e 49
igor_v 42:6fc307c4963e 50 if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль
igor_v 42:6fc307c4963e 51 if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль
igor_v 42:6fc307c4963e 52
igor_v 42:6fc307c4963e 53
igor_v 42:6fc307c4963e 54 Buff_1Point[CountV255] = Dif_QEI;// накопление в буфер еденичных значений приращения по каждому такту.
igor_v 42:6fc307c4963e 55
igor_v 42:6fc307c4963e 56 ////////////////////////////////////////////////////////////////////////////////////////////
igor_v 42:6fc307c4963e 57 ///////////////////////////////////////////////////////////////////////////////////////////
igor_v 42:6fc307c4963e 58
igor_v 42:6fc307c4963e 59 // зачем это. мож к этому воремени стали вращатся назад
igor_v 42:6fc307c4963e 60
igor_v 42:6fc307c4963e 61 if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+"
igor_v 42:6fc307c4963e 62 Gyro.Cnt_Mns = Dif_QEI;
igor_v 42:6fc307c4963e 63 } else {
igor_v 42:6fc307c4963e 64 Gyro.Cnt_Pls = -Dif_QEI;
igor_v 42:6fc307c4963e 65 }
igor_v 42:6fc307c4963e 66
igor_v 42:6fc307c4963e 67
igor_v 43:3f7c8a0ea59c 68 // внимание , все расчеты в знаковых целых
igor_v 43:3f7c8a0ea59c 69
igor_v 42:6fc307c4963e 70
igor_v 42:6fc307c4963e 71 ////////// ???? Заплатка
igor_v 42:6fc307c4963e 72 ////////// ???? Проблема начального заполнения буферов
igor_v 42:6fc307c4963e 73 //////////////////////////////////////////////////////////////////////////////////////////////
igor_v 42:6fc307c4963e 74 // если прошло 32 такта вибро подвеса начать заполнять остальные буверы.
igor_v 42:6fc307c4963e 75 Pulse_8Point += Buff_1Point[CountV255];
igor_v 43:3f7c8a0ea59c 76 Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов
igor_v 42:6fc307c4963e 77 Buff_8Point[CountV31] =Pulse_8Point;
igor_v 42:6fc307c4963e 78
igor_v 42:6fc307c4963e 79 Pulse_16Point += Buff_1Point[CountV255];
igor_v 43:3f7c8a0ea59c 80 Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов
igor_v 42:6fc307c4963e 81 Buff_16Point[CountV31] = Pulse_16Point;
igor_v 42:6fc307c4963e 82
igor_v 42:6fc307c4963e 83 Pulse_32Point += Buff_1Point[CountV255];
igor_v 42:6fc307c4963e 84 Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов
igor_v 42:6fc307c4963e 85 Buff_32Point[CountV31] = Pulse_32Point;
igor_v 42:6fc307c4963e 86
igor_v 42:6fc307c4963e 87 Pulse_16PointD += Buff_1Point[CountV255];
igor_v 42:6fc307c4963e 88 Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз
igor_v 42:6fc307c4963e 89 Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; //
igor_v 42:6fc307c4963e 90 Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // з
igor_v 43:3f7c8a0ea59c 91 Buff_16PointD[CountV31] = Pulse_16PointD;
igor_v 43:3f7c8a0ea59c 92 Buff_Restored_sin [CountV31]= Buff_16PointD [ CountV31] - Buff_32Point[CountV31]; //Востанавливаем синус (для работы вибропривода)
igor_v 42:6fc307c4963e 93
igor_v 43:3f7c8a0ea59c 94 Gyro.Cnt_Dif = (Pulse_32Point);
igor_v 42:6fc307c4963e 95
igor_v 43:3f7c8a0ea59c 96 if(Buff_Restored_sin [CountV31] > 0)
igor_v 43:3f7c8a0ea59c 97 Temp_F_ras += Buff_Restored_sin [CountV31];
igor_v 43:3f7c8a0ea59c 98 else Temp_F_ras -= Buff_Restored_sin [CountV31]; // расчет амплитуды
igor_v 42:6fc307c4963e 99
igor_v 43:3f7c8a0ea59c 100 if (CountV31 == 0) { // если определеный так, то переписываем амиплитуду
igor_v 43:3f7c8a0ea59c 101 Gyro.F_ras = Temp_F_ras;
igor_v 43:3f7c8a0ea59c 102 Temp_F_ras = 0;
igor_v 43:3f7c8a0ea59c 103 }
igor_v 43:3f7c8a0ea59c 104 // зачем это.
igor_v 42:6fc307c4963e 105
igor_v 42:6fc307c4963e 106
igor_v 42:6fc307c4963e 107
Kovalev_D 45:cbc955aecebe 108 /* if(Buff_32Point[CountV255]>0) {
igor_v 42:6fc307c4963e 109 Temp_F_ras += Buff_32Point[CountV255];
igor_v 42:6fc307c4963e 110 } else if ((CountV255 & 0x1f)==0) {
igor_v 42:6fc307c4963e 111 Gyro.F_ras=Temp_F_ras;
igor_v 42:6fc307c4963e 112 Temp_F_ras=0;
igor_v 42:6fc307c4963e 113 } else {
igor_v 42:6fc307c4963e 114 Temp_F_ras -= Buff_32Point[CountV255];
igor_v 42:6fc307c4963e 115 }
igor_v 42:6fc307c4963e 116
Kovalev_D 45:cbc955aecebe 117 */
igor_v 42:6fc307c4963e 118 }
igor_v 42:6fc307c4963e 119
igor_v 42:6fc307c4963e 120
igor_v 42:6fc307c4963e 121
igor_v 21:bc8c1cec3da6 122 void D_QEI(void)
igor_v 21:bc8c1cec3da6 123 {
igor_v 21:bc8c1cec3da6 124 Dif_QEI=0;
igor_v 21:bc8c1cec3da6 125 Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
igor_v 21:bc8c1cec3da6 126 Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.()
igor_v 21:bc8c1cec3da6 127 Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения.
igor_v 21:bc8c1cec3da6 128
igor_v 21:bc8c1cec3da6 129 if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль
igor_v 21:bc8c1cec3da6 130 if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль
igor_v 0:8ad47e2b6f00 131
igor_v 0:8ad47e2b6f00 132
Kovalev_D 45:cbc955aecebe 133 Buff_1Point[CountV255] = /*(unsigned int)*/ (Dif_QEI /* + 0xffff*/ );// накопление в буфер еденичных значений приращения по каждому такту.
igor_v 21:bc8c1cec3da6 134
igor_v 21:bc8c1cec3da6 135 ////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 136 ///////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 137
igor_v 21:bc8c1cec3da6 138 if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+" сторону
igor_v 30:17c84ed091b3 139 Gyro.Cnt_Mns = Dif_QEI;
igor_v 21:bc8c1cec3da6 140 } else {
igor_v 30:17c84ed091b3 141 Gyro.Cnt_Pls = -Dif_QEI;
igor_v 21:bc8c1cec3da6 142 }
igor_v 42:6fc307c4963e 143
igor_v 21:bc8c1cec3da6 144 ////////// ???? Заплатка
igor_v 21:bc8c1cec3da6 145 ////////// ???? Проблема начального заполнения буферов
igor_v 21:bc8c1cec3da6 146 //////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 147 if(FFF==1) { // если прошло 32 такта вибро подвеса начать заполнять остальные буверы.
igor_v 21:bc8c1cec3da6 148 Pulse_8Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 149 Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов
Kovalev_D 45:cbc955aecebe 150 Buff_8Point[CountV255] = /*(unsigned int)*/ (Pulse_8Point/* + 0xffff*/);
igor_v 0:8ad47e2b6f00 151
igor_v 21:bc8c1cec3da6 152 Pulse_16Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 153 Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов
Kovalev_D 45:cbc955aecebe 154 Buff_16Point[CountV255] = /*(unsigned int)*/ (Pulse_16Point /*+ 0xffff*/);
igor_v 21:bc8c1cec3da6 155
igor_v 21:bc8c1cec3da6 156 Pulse_32Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 157 Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов
Kovalev_D 45:cbc955aecebe 158 Gyro.Cnt_Dif = (Pulse_32Point/*+ 0xffff*/);
Kovalev_D 45:cbc955aecebe 159 Buff_32Point[CountV255] = /*(unsigned int)*/ (Pulse_32Point /*+ 0xffff*/);
igor_v 0:8ad47e2b6f00 160
igor_v 21:bc8c1cec3da6 161 Pulse_16PointD += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 162 Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз
igor_v 21:bc8c1cec3da6 163 Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; //
igor_v 21:bc8c1cec3da6 164 Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // з
igor_v 21:bc8c1cec3da6 165
Kovalev_D 45:cbc955aecebe 166 Buff_16PointD[CountV255] = /*(unsigned int)*/ (Pulse_16PointD /*+ 0xffff*/);
igor_v 21:bc8c1cec3da6 167
igor_v 21:bc8c1cec3da6 168
igor_v 0:8ad47e2b6f00 169 // Buff_Restored_sin [CountVf]= (unsigned int)( ((Buff_16Point[CountVf]*2)) - Buff_32Point[CountVf] );
igor_v 0:8ad47e2b6f00 170
Kovalev_D 45:cbc955aecebe 171 Buff_Restored_sin [CountV255]= /*(unsigned int)*/( Buff_16PointD [ CountV255] /*+ 65536 */ - Buff_32Point[CountV255] );
igor_v 21:bc8c1cec3da6 172
igor_v 0:8ad47e2b6f00 173
igor_v 0:8ad47e2b6f00 174
igor_v 21:bc8c1cec3da6 175 if(Buff_32Point[CountV255]>0) {
igor_v 21:bc8c1cec3da6 176 Temp_F_ras += Buff_32Point[CountV255];
igor_v 21:bc8c1cec3da6 177 } else if ((CountV255 & 0x1f)==0) {
igor_v 30:17c84ed091b3 178 Gyro.F_ras=Temp_F_ras;
igor_v 21:bc8c1cec3da6 179 Temp_F_ras=0;
igor_v 21:bc8c1cec3da6 180 } else {
igor_v 21:bc8c1cec3da6 181 Temp_F_ras -= Buff_32Point[CountV255];
igor_v 21:bc8c1cec3da6 182 }
igor_v 21:bc8c1cec3da6 183
igor_v 21:bc8c1cec3da6 184 //unsigned int T_Vib;
igor_v 0:8ad47e2b6f00 185
igor_v 21:bc8c1cec3da6 186 } else if(CountV255 == 255) {
igor_v 21:bc8c1cec3da6 187 FFF=1;//временная заплатка для паузы перед заполнением остальных буферов
Kovalev_D 45:cbc955aecebe 188 for (yy = 0; yy < 31; yy++ ) {
igor_v 21:bc8c1cec3da6 189 Buff_1Point [yy] = 0xffff;
igor_v 21:bc8c1cec3da6 190 Buff_16Point [yy] = 0xffff;
igor_v 21:bc8c1cec3da6 191 Buff_32Point [yy] = 0xffff;
igor_v 21:bc8c1cec3da6 192 Buff_16PointD [yy] = 0xffff;
igor_v 21:bc8c1cec3da6 193 }
igor_v 0:8ad47e2b6f00 194
igor_v 21:bc8c1cec3da6 195 }
igor_v 21:bc8c1cec3da6 196 }