123
Fork of LG by
Diff: DeviceQEI.c
- Revision:
- 198:78dd6d14d108
- Parent:
- 197:7a05523bf588
diff -r 7a05523bf588 -r 78dd6d14d108 DeviceQEI.c --- a/DeviceQEI.c Wed Oct 19 10:55:05 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,57 +0,0 @@ -#include "Device.h" -extern Device device; - -//#define MAX_QEI_CNT 2147483646 - -void InitQEIDefaultSettings(void){ -} - -void InitQEIState(void){ -} - -void DeviceStartQEI(void){ - //Power on - LPC_SC->PCONP |= (1<<18); - //Clock - LPC_SC->PCLKSEL1 |= 0x00000001; // выбор частоты для кв.э. CLK=100MHz - //Pin function select - LPC_PINCON->PINSEL3 &= ~0x4100; - LPC_PINCON->PINSEL3 |= 0x4100; //P1.20, p1.23 установить как входы кв.э - //Pin mode select - LPC_PINCON->PINMODE3 |= 0x3C300; //P1.20, p1.23 - - //LPC_QEI->QEIMAXPOS = MAX_QEI_CNT; //LPC_QEI->"регистор максимального положения" = 2147483646 - LPC_QEI->MAXPOS = 0xffffffff; // - LPC_QEI->FILTER = 2; // фильтр( ?? ) - LPC_QEI->CON = 0xF; //сбросить в ноль все счетчики кв.э. (скорость, направление, позицию и тд) - LPC_QEI->CONF = (0<<2) |(0<<1); //Quadrature inputs, no inverting,only A pulses are counted - - LPC_QEI->CLR = 0x1fff; //r. сбросить все прерывания - LPC_QEI->IEC = 0x1fff; //r. запретить прерывание при изменении направления. TODO: meander mode!!! -} - -#ifdef DEBUG_QEI -void qeiProcess(void) { - static uint16_t idx = 0; - static int16_t deltas[] = {0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25, 0x27, 0x27, 0x25, 0x22, 0x1e, 0x18, 0x12, 0xb, 0x3, -0x3, -0xb, -0x12, -0x18, -0x1e, -0x22, -0x25, -0x27, -0x27, -0x25, -0x22, -0x1e, -0x18, -0x12, -0xb, -0x3, 0x3, 0xb, 0x12, 0x18, 0x1e, 0x22, 0x25}; - device.controller.QEI.state.delta = deltas[idx]; - idx++; if (idx == 1536) idx = 0; //48 cycles -} -#else -void qeiProcess(void) -{ - //TODO: zero QEI position instead of difference??? - // "+": no QEI position overflow - //device.controller.QEI.state.delta = LPC_QEI->QEIPOS; //Get delta directly - //LPC_QEI->QEICON = 0xF; //Zero QEI counters - - int32_t position = LPC_QEI->POS; - device.controller.QEI.state.delta = position - device.controller.QEI.state.position; - device.controller.QEI.state.position = position; - - //???????? - //if (device.controller.QEI.state.delta < -0xfff) device.controller.QEI.state.delta += 0x10000; // обработка прохода значения через ноль - //if (device.controller.QEI.state.delta > 0xfff) device.controller.QEI.state.delta -= 0x10000; // обработка прохода значения через ноль - //???????? -} -#endif \ No newline at end of file