Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

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?

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 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 }