![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fork
Fork of LG by
QEI.c@30:17c84ed091b3, 2016-02-05 (annotated)
- Committer:
- igor_v
- Date:
- Fri Feb 05 23:21:54 2016 +0000
- Revision:
- 30:17c84ed091b3
- Parent:
- 21:bc8c1cec3da6
- Child:
- 42:6fc307c4963e
1
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 | 21:bc8c1cec3da6 | 8 | int FFF=0;//для заплатки |
igor_v | 0:8ad47e2b6f00 | 9 | |
igor_v | 21:bc8c1cec3da6 | 10 | int Pulse_8Point = 0; |
igor_v | 21:bc8c1cec3da6 | 11 | int Pulse_16Point = 0; |
igor_v | 21:bc8c1cec3da6 | 12 | int Pulse_32Point = 0; |
igor_v | 21:bc8c1cec3da6 | 13 | int Pulse_16PointD = 0; |
igor_v | 21:bc8c1cec3da6 | 14 | unsigned int Iras=0,Temp_F_ras=0; |
igor_v | 21:bc8c1cec3da6 | 15 | int yy = 0; |
igor_v | 0:8ad47e2b6f00 | 16 | |
igor_v | 0:8ad47e2b6f00 | 17 | |
igor_v | 21:bc8c1cec3da6 | 18 | unsigned int Buff_32Point [256]; |
igor_v | 21:bc8c1cec3da6 | 19 | unsigned int Buff_16Point [256]; |
igor_v | 21:bc8c1cec3da6 | 20 | unsigned int Buff_16PointD [256]; |
igor_v | 21:bc8c1cec3da6 | 21 | unsigned int Buff_8Point [256]; |
igor_v | 21:bc8c1cec3da6 | 22 | unsigned int Buff_1Point [256]; |
igor_v | 21:bc8c1cec3da6 | 23 | unsigned int Buff_Restored_sin [256]; |
igor_v | 21:bc8c1cec3da6 | 24 | int unsigned Cur_QEI = 0, Last_QEI=0; |
igor_v | 21:bc8c1cec3da6 | 25 | |
igor_v | 21:bc8c1cec3da6 | 26 | |
igor_v | 21:bc8c1cec3da6 | 27 | void D_QEI(void) |
igor_v | 21:bc8c1cec3da6 | 28 | { |
igor_v | 21:bc8c1cec3da6 | 29 | Dif_QEI=0; |
igor_v | 21:bc8c1cec3da6 | 30 | |
igor_v | 21:bc8c1cec3da6 | 31 | |
igor_v | 21:bc8c1cec3da6 | 32 | Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера. |
igor_v | 21:bc8c1cec3da6 | 33 | Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.() |
igor_v | 21:bc8c1cec3da6 | 34 | Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения. |
igor_v | 21:bc8c1cec3da6 | 35 | |
igor_v | 21:bc8c1cec3da6 | 36 | |
igor_v | 21:bc8c1cec3da6 | 37 | if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль |
igor_v | 21:bc8c1cec3da6 | 38 | if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль |
igor_v | 0:8ad47e2b6f00 | 39 | |
igor_v | 0:8ad47e2b6f00 | 40 | |
igor_v | 21:bc8c1cec3da6 | 41 | Buff_1Point[CountV255] = (unsigned int) (Dif_QEI + 0xffff);// накопление в буфер еденичных значений приращения по каждому такту. |
igor_v | 21:bc8c1cec3da6 | 42 | |
igor_v | 21:bc8c1cec3da6 | 43 | //////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 44 | /////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 45 | |
igor_v | 21:bc8c1cec3da6 | 46 | if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+" сторону |
igor_v | 30:17c84ed091b3 | 47 | Gyro.Cnt_Mns = Dif_QEI; |
igor_v | 21:bc8c1cec3da6 | 48 | } else { |
igor_v | 30:17c84ed091b3 | 49 | Gyro.Cnt_Pls = -Dif_QEI; |
igor_v | 21:bc8c1cec3da6 | 50 | } |
igor_v | 21:bc8c1cec3da6 | 51 | ////////// ???? Заплатка |
igor_v | 21:bc8c1cec3da6 | 52 | ////////// ???? Проблема начального заполнения буферов |
igor_v | 21:bc8c1cec3da6 | 53 | ////////////////////////////////////////////////////////////////////////////////////////////// |
igor_v | 21:bc8c1cec3da6 | 54 | if(FFF==1) { // если прошло 32 такта вибро подвеса начать заполнять остальные буверы. |
igor_v | 21:bc8c1cec3da6 | 55 | Pulse_8Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 56 | Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов |
igor_v | 21:bc8c1cec3da6 | 57 | Buff_8Point[CountV255] = (unsigned int) (Pulse_8Point + 0xffff); |
igor_v | 0:8ad47e2b6f00 | 58 | |
igor_v | 21:bc8c1cec3da6 | 59 | Pulse_16Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 60 | Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов |
igor_v | 21:bc8c1cec3da6 | 61 | Buff_16Point[CountV255] = (unsigned int) (Pulse_16Point + 0xffff); |
igor_v | 21:bc8c1cec3da6 | 62 | |
igor_v | 21:bc8c1cec3da6 | 63 | Pulse_32Point += Buff_1Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 64 | Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов |
igor_v | 30:17c84ed091b3 | 65 | Gyro.Cnt_Dif = (Pulse_32Point+ 0xffff); |
igor_v | 21:bc8c1cec3da6 | 66 | Buff_32Point[CountV255] = (unsigned int) (Pulse_32Point + 0xffff); |
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]; // |
igor_v | 21:bc8c1cec3da6 | 71 | Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // з |
igor_v | 21:bc8c1cec3da6 | 72 | |
igor_v | 21:bc8c1cec3da6 | 73 | Buff_16PointD[CountV255] = (unsigned int) (Pulse_16PointD + 0xffff); |
igor_v | 21:bc8c1cec3da6 | 74 | |
igor_v | 21:bc8c1cec3da6 | 75 | |
igor_v | 0:8ad47e2b6f00 | 76 | // Buff_Restored_sin [CountVf]= (unsigned int)( ((Buff_16Point[CountVf]*2)) - Buff_32Point[CountVf] ); |
igor_v | 0:8ad47e2b6f00 | 77 | |
igor_v | 21:bc8c1cec3da6 | 78 | Buff_Restored_sin [CountV255]= (unsigned int)( Buff_16PointD [ CountV255] + 65536 - Buff_32Point[CountV255] ); |
igor_v | 21:bc8c1cec3da6 | 79 | |
igor_v | 0:8ad47e2b6f00 | 80 | |
igor_v | 0:8ad47e2b6f00 | 81 | |
igor_v | 21:bc8c1cec3da6 | 82 | if(Buff_32Point[CountV255]>0) { |
igor_v | 21:bc8c1cec3da6 | 83 | Temp_F_ras += Buff_32Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 84 | } else if ((CountV255 & 0x1f)==0) { |
igor_v | 30:17c84ed091b3 | 85 | Gyro.F_ras=Temp_F_ras; |
igor_v | 21:bc8c1cec3da6 | 86 | Temp_F_ras=0; |
igor_v | 21:bc8c1cec3da6 | 87 | } else { |
igor_v | 21:bc8c1cec3da6 | 88 | Temp_F_ras -= Buff_32Point[CountV255]; |
igor_v | 21:bc8c1cec3da6 | 89 | } |
igor_v | 21:bc8c1cec3da6 | 90 | |
igor_v | 21:bc8c1cec3da6 | 91 | //unsigned int T_Vib; |
igor_v | 0:8ad47e2b6f00 | 92 | |
igor_v | 21:bc8c1cec3da6 | 93 | } else if(CountV255 == 255) { |
igor_v | 21:bc8c1cec3da6 | 94 | FFF=1;//временная заплатка для паузы перед заполнением остальных буферов |
igor_v | 21:bc8c1cec3da6 | 95 | for (yy = 0; yy < 256; yy++ ) { |
igor_v | 21:bc8c1cec3da6 | 96 | Buff_1Point [yy] = 0xffff; |
igor_v | 21:bc8c1cec3da6 | 97 | Buff_16Point [yy] = 0xffff; |
igor_v | 21:bc8c1cec3da6 | 98 | Buff_32Point [yy] = 0xffff; |
igor_v | 21:bc8c1cec3da6 | 99 | Buff_16PointD [yy] = 0xffff; |
igor_v | 21:bc8c1cec3da6 | 100 | } |
igor_v | 0:8ad47e2b6f00 | 101 | |
igor_v | 21:bc8c1cec3da6 | 102 | } |
igor_v | 21:bc8c1cec3da6 | 103 | } |