Diff: DeviceQEI.c

Revision:

198:78dd6d14d108

Parent:

197:7a05523bf588

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