Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

QEI.c@98:95b8e79f13e1, 2016-03-29 (annotated)

Committer:
Kovalev_D
Date:
Tue Mar 29 09:51:17 2016 +0000
Revision:
98:95b8e79f13e1
Parent:
92:c892f0311aa7
Child:
105:bd01d8d20fb6
456

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
Kovalev_D 86:398da56ef751 4
Kovalev_D 86:398da56ef751 5
Kovalev_D 86:398da56ef751 6 int CuruAngle ;
Kovalev_D 86:398da56ef751 7
igor_v 21:bc8c1cec3da6 8 int Dif_QEI;
igor_v 42:6fc307c4963e 9
igor_v 21:bc8c1cec3da6 10 int FFF=0;//для заплатки
igor_v 0:8ad47e2b6f00 11
igor_v 42:6fc307c4963e 12
Kovalev_D 86:398da56ef751 13 int Pulse_8Point;
Kovalev_D 86:398da56ef751 14 int Pulse_16Point;
Kovalev_D 86:398da56ef751 15 int Pulse_32Point;
Kovalev_D 86:398da56ef751 16 int Pulse_16PointD;
igor_v 21:bc8c1cec3da6 17 unsigned int Iras=0,Temp_F_ras=0;
igor_v 21:bc8c1cec3da6 18 int yy = 0;
igor_v 0:8ad47e2b6f00 19
igor_v 0:8ad47e2b6f00 20
igor_v 42:6fc307c4963e 21
igor_v 42:6fc307c4963e 22
Kovalev_D 86:398da56ef751 23 int Buff_32Point [32];
igor_v 43:3f7c8a0ea59c 24 int Buff_16Point [32];
igor_v 43:3f7c8a0ea59c 25 int Buff_16PointD [32];
igor_v 43:3f7c8a0ea59c 26 int Buff_8Point [32];
igor_v 43:3f7c8a0ea59c 27 int Buff_Restored_sin [32];
igor_v 43:3f7c8a0ea59c 28 int Buff_1Point [256]; // этот на 256
igor_v 42:6fc307c4963e 29
igor_v 42:6fc307c4963e 30
igor_v 42:6fc307c4963e 31
Kovalev_D 86:398da56ef751 32 int unsigned Cur_QEI, Last_QEI;
igor_v 21:bc8c1cec3da6 33
igor_v 43:3f7c8a0ea59c 34 // ДАВАЙ попробуем сделать буфера в целых (знаковы)
Kovalev_D 98:95b8e79f13e1 35 /*__irq void QEI_IRQHandler (void)
Kovalev_D 98:95b8e79f13e1 36 {
Kovalev_D 98:95b8e79f13e1 37 Cur_QEI = LPC_QEI->POS & 0xFFFF; //e. read accumulated value of counter
Kovalev_D 98:95b8e79f13e1 38 LPC_QEI->CLR = 0x1fff; //e. reset interrupt request //r. сбросить запрос прерывания
Kovalev_D 98:95b8e79f13e1 39 }*/
Kovalev_D 86:398da56ef751 40
igor_v 21:bc8c1cec3da6 41 void D_QEI(void)
igor_v 21:bc8c1cec3da6 42 {
igor_v 21:bc8c1cec3da6 43 Dif_QEI=0;
igor_v 21:bc8c1cec3da6 44 Cur_QEI = LPC_QEI->POS & 0xFFFF; // считывание текущего значения энкодера.
igor_v 21:bc8c1cec3da6 45 Dif_QEI = (Cur_QEI - Last_QEI); // получение приращения.()
igor_v 21:bc8c1cec3da6 46 Last_QEI = Cur_QEI; // запись текущего значения энкодера в регистр предыдущего значения.
igor_v 21:bc8c1cec3da6 47
igor_v 21:bc8c1cec3da6 48 if (Dif_QEI < -0xfff) Dif_QEI += 0x10000; // обработка прохода значения через ноль
igor_v 21:bc8c1cec3da6 49 if (Dif_QEI > 0xfff) Dif_QEI -= 0x10000; // обработка прохода значения через ноль
igor_v 0:8ad47e2b6f00 50
igor_v 0:8ad47e2b6f00 51
Kovalev_D 45:cbc955aecebe 52 Buff_1Point[CountV255] = /*(unsigned int)*/ (Dif_QEI /* + 0xffff*/ );// накопление в буфер еденичных значений приращения по каждому такту.
igor_v 21:bc8c1cec3da6 53
igor_v 21:bc8c1cec3da6 54 ////////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 55 ///////////////////////////////////////////////////////////////////////////////////////////
igor_v 21:bc8c1cec3da6 56
igor_v 21:bc8c1cec3da6 57 if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+" сторону
igor_v 30:17c84ed091b3 58 Gyro.Cnt_Mns = Dif_QEI;
igor_v 21:bc8c1cec3da6 59 } else {
igor_v 30:17c84ed091b3 60 Gyro.Cnt_Pls = -Dif_QEI;
igor_v 21:bc8c1cec3da6 61 }
igor_v 42:6fc307c4963e 62
igor_v 21:bc8c1cec3da6 63 ////////// ???? Заплатка
igor_v 21:bc8c1cec3da6 64 ////////// ???? Проблема начального заполнения буферов
igor_v 21:bc8c1cec3da6 65 //////////////////////////////////////////////////////////////////////////////////////////////
Kovalev_D 98:95b8e79f13e1 66 if(1)// if(FFF==1) { // если прошло 32 такта вибро подвеса начать заполнять остальные буверы.
Kovalev_D 98:95b8e79f13e1 67 { Pulse_8Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 68 Pulse_8Point -= Buff_1Point[(CountV255-8) & 0xff]; // заполнение буфера накопленых приращений за 8 тактов
Kovalev_D 92:c892f0311aa7 69 Buff_8Point[CountV31] = /*(unsigned int)*/ (Pulse_8Point/* + 0xffff*/);
igor_v 0:8ad47e2b6f00 70
igor_v 21:bc8c1cec3da6 71 Pulse_16Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 72 Pulse_16Point -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов
Kovalev_D 92:c892f0311aa7 73 Buff_16Point[CountV31] = /*(unsigned int)*/ (Pulse_16Point /*+ 0xffff*/);
igor_v 21:bc8c1cec3da6 74
igor_v 21:bc8c1cec3da6 75 Pulse_32Point += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 76 Pulse_32Point -= Buff_1Point[(CountV255-32) & 0xff]; // заполнение буфера накопленых приращений за 32 тактов
Kovalev_D 45:cbc955aecebe 77 Gyro.Cnt_Dif = (Pulse_32Point/*+ 0xffff*/);
Kovalev_D 92:c892f0311aa7 78 Buff_32Point[CountV31] = /*(unsigned int)*/ (Pulse_32Point /*+ 0xffff*/);
igor_v 0:8ad47e2b6f00 79
igor_v 21:bc8c1cec3da6 80 Pulse_16PointD += Buff_1Point[CountV255];
igor_v 21:bc8c1cec3da6 81 Pulse_16PointD -= Buff_1Point[(CountV255-16) & 0xff]; // заполнение буфера накопленых приращений за 16 тактов Двойныз
igor_v 21:bc8c1cec3da6 82 Pulse_16PointD += Buff_1Point[(CountV255-32) & 0xff]; //
igor_v 21:bc8c1cec3da6 83 Pulse_16PointD -= Buff_1Point[(CountV255-48) & 0xff]; // з
igor_v 21:bc8c1cec3da6 84
Kovalev_D 92:c892f0311aa7 85 Buff_16PointD[CountV31] = /*(unsigned int)*/ (Pulse_16PointD /*+ 0xffff*/);
igor_v 21:bc8c1cec3da6 86
Kovalev_D 92:c892f0311aa7 87 Buff_Restored_sin [CountV31]= /*(unsigned int)*/( Buff_16PointD [ CountV31] /*+ 65536 */ - Buff_32Point[CountV31] );
igor_v 21:bc8c1cec3da6 88
igor_v 0:8ad47e2b6f00 89
igor_v 0:8ad47e2b6f00 90
Kovalev_D 87:7e575d26d6d0 91 if(Buff_Restored_sin [CountV31] > 0)
Kovalev_D 87:7e575d26d6d0 92 Temp_F_ras += Buff_Restored_sin [CountV31];
Kovalev_D 87:7e575d26d6d0 93 else Temp_F_ras -= Buff_Restored_sin [CountV31]; // расчет амплитуды
igor_v 21:bc8c1cec3da6 94
Kovalev_D 87:7e575d26d6d0 95 if (CountV31 == 0) { // если определеный так, то переписываем амиплитуду
Kovalev_D 87:7e575d26d6d0 96 Gyro.F_ras = Temp_F_ras;
Kovalev_D 87:7e575d26d6d0 97 Temp_F_ras = 0;
Kovalev_D 87:7e575d26d6d0 98 }
Kovalev_D 87:7e575d26d6d0 99
igor_v 0:8ad47e2b6f00 100
igor_v 21:bc8c1cec3da6 101 } else if(CountV255 == 255) {
igor_v 21:bc8c1cec3da6 102 FFF=1;//временная заплатка для паузы перед заполнением остальных буферов
Kovalev_D 98:95b8e79f13e1 103 // for (yy = 0; yy < 31; yy++ ) {
Kovalev_D 98:95b8e79f13e1 104 /// Buff_1Point [yy] = 0x00;
Kovalev_D 98:95b8e79f13e1 105 // Buff_16Point [yy] = 0x00;
Kovalev_D 98:95b8e79f13e1 106 // Buff_32Point [yy] = 0x00;
Kovalev_D 98:95b8e79f13e1 107 // Buff_16PointD [yy] = 0x0;
Kovalev_D 98:95b8e79f13e1 108 // }
igor_v 0:8ad47e2b6f00 109
igor_v 21:bc8c1cec3da6 110 }
igor_v 21:bc8c1cec3da6 111 }