Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
QEI.c@45:cbc955aecebe, 2016-02-09 (annotated)
- Committer:
- Kovalev_D
- Date:
- Tue Feb 09 13:47:26 2016 +0000
- Revision:
- 45:cbc955aecebe
- Parent:
- 43:3f7c8a0ea59c
- Child:
- 86:398da56ef751
unsignet int-> int
Who changed what in which revision?
User | Revision | Line number | New 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 | |
Kovalev_D | 45:cbc955aecebe | 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 | |
Kovalev_D | 45:cbc955aecebe | 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 | |
Kovalev_D | 45:cbc955aecebe | 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 тактов |
Kovalev_D | 45:cbc955aecebe | 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 тактов |
Kovalev_D | 45:cbc955aecebe | 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 тактов |
Kovalev_D | 45:cbc955aecebe | 165 | Gyro.Cnt_Dif = (Pulse_32Point/*+ 0xffff*/); |
Kovalev_D | 45:cbc955aecebe | 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 | |
Kovalev_D | 45:cbc955aecebe | 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 | |
Kovalev_D | 45:cbc955aecebe | 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;//временная заплатка для паузы перед заполнением остальных буферов |
Kovalev_D | 45:cbc955aecebe | 195 | for (yy = 0; yy < 31; 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 | } |