QEI.c

00001 #include "Global.h"
00002 
00003 
00004 int Pulse_midl = 0;
00005 int PulseHalf = 0;
00006 int CuruAngle = 0;
00007 int Dif_QEI;
00008 int FFF=0;//для заплатки
00009 
00010 int Pulse_8Point = 0;
00011 int Pulse_16Point = 0;
00012 int Pulse_32Point = 0;
00013 int Pulse_16PointD = 0;
00014 unsigned int Iras=0,Temp_F_ras=0;
00015 int yy = 0;
00016 
00017 
00018 unsigned int Buff_32Point          [256];
00019 unsigned int Buff_16Point          [256];
00020 unsigned int Buff_16PointD     [256];
00021 unsigned int Buff_8Point         [256];
00022 unsigned int Buff_1Point           [256];
00023 unsigned int Buff_Restored_sin [256];
00024 int unsigned Cur_QEI = 0, Last_QEI=0;
00025 
00026 
00027 void D_QEI(void)
00028 {
00029     Dif_QEI=0;
00030 
00031 
00032     Cur_QEI  =  LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
00033     Dif_QEI  =  (Cur_QEI - Last_QEI);  // получение приращения.()
00034     Last_QEI =  Cur_QEI;               // запись текущего значения энкодера в регистр предыдущего значения.
00035 
00036 
00037     if (Dif_QEI < -0xfff)  Dif_QEI += 0x10000;   // обработка прохода значения через ноль
00038     if (Dif_QEI >  0xfff)  Dif_QEI -= 0x10000;    // обработка прохода значения через ноль
00039 
00040 
00041     Buff_1Point[CountV255] = (unsigned int) (Dif_QEI + 0xffff);// накопление в буфер еденичных значений приращения по каждому такту.
00042 
00043     ////////////////////////////////////////////////////////////////////////////////////////////
00044     ///////////////////////////////////////////////////////////////////////////////////////////
00045 
00046     if (LPC_QEI->STAT) {                    //e. "+" direction //r. стали вращаться в "+" сторону
00047         Main.Cnt_Mns = Dif_QEI;
00048     } else {
00049         Main.Cnt_Pls = -Dif_QEI;
00050     }
00051     ////////// ???? Заплатка
00052     ////////// ???? Проблема начального заполнения буферов
00053     //////////////////////////////////////////////////////////////////////////////////////////////
00054     if(FFF==1) { // если прошло 32 такта вибро подвеса начать заполнять остальные буверы.
00055         Pulse_8Point += Buff_1Point[CountV255];
00056         Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff];                    // заполнение буфера накопленых приращений за 8 тактов
00057         Buff_8Point[CountV255] = (unsigned int) (Pulse_8Point + 0xffff);
00058 
00059         Pulse_16Point += Buff_1Point[CountV255];
00060         Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff];                                    // заполнение буфера накопленых приращений за 16 тактов
00061         Buff_16Point[CountV255] = (unsigned int) (Pulse_16Point + 0xffff);
00062 
00063         Pulse_32Point += Buff_1Point[CountV255];
00064         Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff];                                // заполнение буфера накопленых приращений за 32 тактов
00065         Main.Cnt_Dif  =  (Pulse_32Point+ 0xffff);
00066         Buff_32Point[CountV255] = (unsigned int) (Pulse_32Point + 0xffff);
00067 
00068         Pulse_16PointD += Buff_1Point[CountV255];
00069         Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff];                                   // заполнение буфера накопленых приращений за 16 тактов Двойныз
00070         Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff];                                   //
00071         Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff];                                   // з
00072 
00073         Buff_16PointD[CountV255] = (unsigned int) (Pulse_16PointD + 0xffff);
00074 
00075 
00076 //                          Buff_Restored_sin [CountVf]= (unsigned int)( ((Buff_16Point[CountVf]*2)) - Buff_32Point[CountVf] );
00077 
00078         Buff_Restored_sin [CountV255]= (unsigned int)( Buff_16PointD [ CountV255] + 65536 - Buff_32Point[CountV255] );
00079 
00080 
00081 
00082         if(Buff_32Point[CountV255]>0) {
00083             Temp_F_ras += Buff_32Point[CountV255];
00084         } else if ((CountV255 & 0x1f)==0) {
00085             Main.F_ras=Temp_F_ras;
00086             Temp_F_ras=0;
00087         } else {
00088             Temp_F_ras -=  Buff_32Point[CountV255];
00089         }
00090 
00091         //unsigned int T_Vib;
00092 
00093     } else if(CountV255 == 255) {
00094         FFF=1;//временная заплатка  для паузы перед заполнением остальных буферов
00095         for (yy = 0; yy < 256; yy++ ) {
00096             Buff_1Point [yy] = 0xffff;
00097             Buff_16Point [yy] = 0xffff;
00098             Buff_32Point [yy] = 0xffff;
00099             Buff_16PointD [yy] = 0xffff;
00100         }
00101 
00102     }
00103 }