Quadrature Encoder Interface for motion control with resistance to jitter and chatter on AB signals and motor vibrations
Dependents: QEIx4_Example realtimeMM_V3 realtimeMM_V3
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 ///////////////////////////////////////////////////////////////////////////////
Generated on Fri Jul 15 2022 02:24:32 by 1.7.2