Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

QEI.c@43:3f7c8a0ea59c, 2016-02-07 (annotated)

Committer:
igor_v
Date:
Sun Feb 07 16:01:14 2016 +0000
Revision:
43:3f7c8a0ea59c
Parent:
42:6fc307c4963e
Child:
45:cbc955aecebe
1

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