QEIx4.cpp

00001 #include "QEIx4.h"
00002 
00003 // bit masks for state machine - don't change!!!
00004 #define QEIx4_STATE 0xC
00005 #define QEIx4_MASK  0x1C
00006 #define QEIx4_INC   0x40
00007 #define QEIx4_DEC   0x80
00008 #define QEIx4_CHG   0xC0
00009 #define QEIx4_DIR   0x20
00010 #define QEIx4_A     1
00011 #define QEIx4_B     2
00012 #define QEIx4_AB    3
00013 #define QEIx4_S0    0x0
00014 #define QEIx4_S1    0x4
00015 #define QEIx4_S2    0x8
00016 #define QEIx4_S3    0xC
00017 #define QEIx4_CCW   0
00018 #define QEIx4_CW    0x10
00019 
00020 // state machine for decoting - don't change!!!
00021 short QEIx4::_modeLUT[32] = {
00022     // act state S0 in CCW direction
00023     QEIx4_CCW | QEIx4_S0,
00024     QEIx4_CW  | QEIx4_S1 | QEIx4_A  | QEIx4_INC | QEIx4_DIR,
00025     QEIx4_CCW | QEIx4_S0 | QEIx4_B,
00026     QEIx4_CCW | QEIx4_S3 | QEIx4_AB | QEIx4_DEC,
00027     // act state S1 in CCW direction
00028     QEIx4_CCW | QEIx4_S1,
00029     QEIx4_CCW | QEIx4_S1 | QEIx4_A,
00030     QEIx4_CCW | QEIx4_S0 | QEIx4_B  | QEIx4_DEC,
00031     QEIx4_CW  | QEIx4_S2 | QEIx4_AB | QEIx4_INC | QEIx4_DIR,
00032     // act state S2 in CCW direction
00033     QEIx4_CCW | QEIx4_S1            | QEIx4_DEC,
00034     QEIx4_CCW | QEIx4_S2 | QEIx4_A,
00035     QEIx4_CW  | QEIx4_S3 | QEIx4_B  | QEIx4_INC | QEIx4_DIR,
00036     QEIx4_CCW | QEIx4_S2 | QEIx4_AB,
00037     // act state S3 in CCW direction
00038     QEIx4_CW  | QEIx4_S0            | QEIx4_INC | QEIx4_DIR,
00039     QEIx4_CCW | QEIx4_S2 | QEIx4_A  | QEIx4_DEC,
00040     QEIx4_CCW | QEIx4_S3 | QEIx4_B,
00041     QEIx4_CCW | QEIx4_S3 | QEIx4_AB,
00042 
00043     // act state S0 in CW direction
00044     QEIx4_CW  | QEIx4_S0,
00045     QEIx4_CW  | QEIx4_S1 | QEIx4_A  | QEIx4_INC,
00046     QEIx4_CW  | QEIx4_S0 | QEIx4_B,
00047     QEIx4_CCW | QEIx4_S3 | QEIx4_AB | QEIx4_DEC | QEIx4_DIR,
00048     // act state S1 in CW direction
00049     QEIx4_CW  | QEIx4_S1,
00050     QEIx4_CW  | QEIx4_S1 | QEIx4_A,
00051     QEIx4_CCW | QEIx4_S0 | QEIx4_B  | QEIx4_DEC | QEIx4_DIR,
00052     QEIx4_CW  | QEIx4_S2 | QEIx4_AB | QEIx4_INC,
00053     // act state S2 in CW direction
00054     QEIx4_CCW | QEIx4_S1            | QEIx4_DEC | QEIx4_DIR,
00055     QEIx4_CW  | QEIx4_S2 | QEIx4_A,
00056     QEIx4_CW  | QEIx4_S3 | QEIx4_B  | QEIx4_INC,
00057     QEIx4_CW  | QEIx4_S2 | QEIx4_AB,
00058     // act state S3 in CW direction
00059     QEIx4_CW  | QEIx4_S0            | QEIx4_INC,
00060     QEIx4_CCW | QEIx4_S2 | QEIx4_A  | QEIx4_DEC | QEIx4_DIR,
00061     QEIx4_CW  | QEIx4_S3 | QEIx4_B,
00062     QEIx4_CW  | QEIx4_S3 | QEIx4_AB
00063 };
00064 
00065 
00066 //#define DEB(x) printf (x)
00067 #define DEB(x)
00068 
00069 ///////////////////////////////////////////////////////////////////////////////
00070 
00071 QEIx4::QEIx4 ( PinName pinA, PinName pinB, PinName pinI, EMODE eMode ) : _pinA(pinA), _pinB(pinB), _pinI(pinI)
00072 {
00073     // prepare input pins
00074     _pinA.mode(PullUp);
00075     _pinB.mode(PullUp);
00076     if(pinI!=NC)
00077         _pinI.mode(PullUp);
00078 
00079     _eMode = eMode;
00080     _bIndexTrigger = false;
00081     _fPositionFactor = 1.0;
00082     _fSpeedFactor = 1.0;
00083     _counter = 0;
00084     _state = 0;
00085     _nSpeedLastTimer = 0;
00086     _nSpeedAvrTimeSum = 0;
00087     _nSpeedAvrTimeCount = -1;
00088     _fLastSpeed = 0;
00089     _nSpeedTimeoutMax = 10;
00090     _nSpeedTimeoutCount = 0;
00091 
00092     // synchronize state machine
00093     if ( _eMode & IRQ ) {
00094         _pinA.fall ( this, &QEIx4::ProcessISR );
00095         _pinA.rise ( this, &QEIx4::ProcessISR );
00096         _pinB.fall ( this, &QEIx4::ProcessISR );
00097         _pinB.rise ( this, &QEIx4::ProcessISR );
00098     }
00099     if ( _eMode & IRQ_NO_JAMMING ) {
00100         ProcessISR ();
00101         _state += QEIx4_S2;
00102     }
00103     ProcessISR ();
00104     _counter = 0;
00105 
00106     // prepare for speed measurement
00107     if ( _eMode & SPEED ) {
00108         _SpeedTimer.reset();
00109         _SpeedTimer.start();
00110     }
00111 }
00112 
00113 ///////////////////////////////////////////////////////////////////////////////
00114 
00115 QEIx4::~QEIx4()
00116 {
00117     _pinA.rise ( NULL );
00118     _pinA.fall ( NULL );
00119     _pinB.rise ( NULL );
00120     _pinB.fall ( NULL );
00121 }
00122 
00123 ///////////////////////////////////////////////////////////////////////////////
00124 
00125 float QEIx4::getSpeed()
00126 {
00127     float fSpeed;
00128     int avrTimeSum = 0;
00129     int avrTimeCount = 0;
00130 
00131     __disable_irq();    // Disable Interrupts for atomic copy
00132     avrTimeSum = _nSpeedAvrTimeSum;
00133     avrTimeCount = _nSpeedAvrTimeCount;
00134     _nSpeedAvrTimeSum = 0;
00135     _nSpeedAvrTimeCount = 0;
00136     __enable_irq();     // Enable Interrupts
00137 
00138     if ( avrTimeCount == 0 ) {
00139         if (_nSpeedTimeoutCount++ > _nSpeedTimeoutMax)
00140             _fLastSpeed *= 0.5f;
00141         fSpeed = _fLastSpeed;
00142     } else if ( avrTimeCount < 0 || avrTimeSum == 0 ) {
00143         fSpeed = 0;
00144         _nSpeedTimeoutCount = 0;
00145     } else {
00146         fSpeed = 1000000.0f * _fSpeedFactor / ( (float)avrTimeSum / (float)avrTimeCount );
00147         _nSpeedTimeoutCount = 0;
00148     }
00149     _fLastSpeed = fSpeed;
00150 
00151     return fSpeed;
00152 }
00153 
00154 ///////////////////////////////////////////////////////////////////////////////
00155 
00156 void QEIx4::ProcessISR ( void )
00157 {
00158     int nLoopCounter=10;
00159     int pinA, pinB;
00160 
00161     do {
00162         pinA = _pinA;
00163         pinB = _pinB;
00164 
00165         DEB (".");
00166         _state &= QEIx4_MASK;
00167         if ( pinA ) _state |= QEIx4_A;
00168         if ( pinB ) _state |= QEIx4_B;
00169 
00170         _state = _modeLUT[_state];   // magic is done by lookup-table
00171 
00172         if ( _state & QEIx4_CHG ) {   // is any change?
00173             bool bCounterChange = false;
00174 
00175             if ( _state & QEIx4_INC ) {   // is moved foreward?
00176                 _counter++;
00177                 bCounterChange = true;
00178             }
00179             if ( _state & QEIx4_DEC ) {   // is moved backward?
00180                 _counter--;
00181                 bCounterChange = true;
00182             }
00183 
00184             if ( _eMode & IRQ_NO_JAMMING ) {   // need reconfiguration of interrupt edges?
00185                 switch ( _state & QEIx4_STATE ) {
00186                     case QEIx4_S0:
00187                         _pinB.rise ( NULL );
00188                         _pinB.fall ( NULL );
00189                         _pinA.rise ( this, &QEIx4::ProcessISR );
00190                         DEB ("S0");
00191                         break;
00192                     case QEIx4_S1:
00193                         _pinA.rise ( NULL );
00194                         _pinA.fall ( NULL );
00195                         _pinB.rise ( this, &QEIx4::ProcessISR );
00196                         DEB ("S1");
00197                         break;
00198                     case QEIx4_S2:
00199                         _pinB.rise ( NULL );
00200                         _pinB.fall ( NULL );
00201                         _pinA.fall ( this, &QEIx4::ProcessISR );
00202                         DEB ("S2");
00203                         break;
00204                     case QEIx4_S3:
00205                         _pinA.rise ( NULL );
00206                         _pinA.fall ( NULL );
00207                         _pinB.fall ( this, &QEIx4::ProcessISR );
00208                         DEB ("S3");
00209                         break;
00210                 }
00211             }
00212 
00213             if ( _eMode & SPEED ) {
00214                 unsigned int act = _SpeedTimer.read_us();
00215                 unsigned int diff = act - _nSpeedLastTimer;   // Note: overflow is handled correctly
00216                 _nSpeedLastTimer = act;
00217 
00218                 if ( _nSpeedAvrTimeCount < 0 ) {   // ignore first pulse to synchronize timer (maybe timer overflow)
00219                     _nSpeedAvrTimeSum = 0;
00220                     _nSpeedAvrTimeCount = 0;
00221                 } else {
00222                     if ( _state & QEIx4_CW )   // is moving foreward?
00223                         _nSpeedAvrTimeSum += diff;
00224                     else
00225                         _nSpeedAvrTimeSum -= diff;
00226                     _nSpeedAvrTimeCount++;
00227                 }
00228             }
00229 
00230             if ( _bIndexTrigger && bCounterChange && _pinI != NC && _pinI == 1 ) {   // is index pin triggered?
00231                 _bIndexTrigger = false;
00232                 fPointerIndexTrigger.call(_counter);
00233             }
00234 
00235             if ( bCounterChange ) {   // has counter changed?
00236                 fPointerCounterChange.call(_counter);
00237                 if ( _state & QEIx4_DIR )
00238                     fPointerDirectionChange.call(_counter);
00239             }
00240 
00241         }
00242     } while (nLoopCounter-->0 && ( pinA != _pinA || pinB != _pinB)); // loop till stable input pins
00243 }
00244 
00245 ///////////////////////////////////////////////////////////////////////////////
00246 ///////////////////////////////////////////////////////////////////////////////
00247 ///////////////////////////////////////////////////////////////////////////////