OpenMoCo / QEIx4

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

Quadrature Encoder Interface for Motion Control.

A class to decode pulses on a rotary encoder with AB signals (quadrature encoder). It uses all 4 edges of the AB signals to increase the counter resolution 4 times of cycles per rotation/revolution (CPR) (e.g. an encoder with 500 CPR get 2000 counts per rotation)

In opposite to most common QEI implementation this is resistant to jitter and chatter on AB signals and motor vibrations. When using interrupts (IRQ_NO_JAMMING-mode) only the needed edge and pin is activated to prevent jamming CPU time with unnecessary interrupts. Whes reaching the next position the edge that triggerd this position (state) is ignored to aboid oscillating up/down counts.

It can also be used in polling mode i.g. in idle routines if interrupts are not desired. At this mode be sure that the sampling frequency is heigher than the maximum rotation speed (expeced counts per second)

The internal state machine is based on a look up table (LUT) to minimize interrupt retention time and get all necessary flags at once.

Additional the rotation speed of the encoder can be measured. The algorithm is based on the measuring time between the edges to get a very precise speed at very slow rotation.

The library is designed to support closed loop speed- and motion-controller for also slow and smooth motions like movie camera motion control.

Quadrature Encoder Signals:

/media/uploads/jocis/qeix4.png

(+) Count UP; (-) Count DOWN

QEIx4.cpp@2:c0b87b11b9cd, 2014-10-01 (annotated)

Committer:
jocis
Date:
Wed Oct 01 10:23:21 2014 +0000
Revision:
2:c0b87b11b9cd
Parent:
1:ac6b7b1bf6c5 
replaced virtual functions by attached functions; added documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jocis 0:46b8d5680f66 1 #include "QEIx4.h"
jocis 0:46b8d5680f66 2
jocis 0:46b8d5680f66 3 // bit masks for state machine - don't change!!!
jocis 1:ac6b7b1bf6c5 4 #define QEIx4_STATE 0xC
jocis 1:ac6b7b1bf6c5 5 #define QEIx4_MASK 0x1C
jocis 1:ac6b7b1bf6c5 6 #define QEIx4_INC 0x40
jocis 1:ac6b7b1bf6c5 7 #define QEIx4_DEC 0x80
jocis 1:ac6b7b1bf6c5 8 #define QEIx4_CHG 0xC0
jocis 1:ac6b7b1bf6c5 9 #define QEIx4_DIR 0x20
jocis 0:46b8d5680f66 10 #define QEIx4_A 1
jocis 0:46b8d5680f66 11 #define QEIx4_B 2
jocis 1:ac6b7b1bf6c5 12 #define QEIx4_AB 3
jocis 0:46b8d5680f66 13 #define QEIx4_S0 0x0
jocis 0:46b8d5680f66 14 #define QEIx4_S1 0x4
jocis 0:46b8d5680f66 15 #define QEIx4_S2 0x8
jocis 0:46b8d5680f66 16 #define QEIx4_S3 0xC
jocis 1:ac6b7b1bf6c5 17 #define QEIx4_CCW 0
jocis 1:ac6b7b1bf6c5 18 #define QEIx4_CW 0x10
jocis 1:ac6b7b1bf6c5 19
jocis 1:ac6b7b1bf6c5 20 // state machine for decoting - don't change!!!
jocis 1:ac6b7b1bf6c5 21 short QEIx4::_modeLUT[32] = {
jocis 1:ac6b7b1bf6c5 22 // act state S0 in CCW direction
jocis 1:ac6b7b1bf6c5 23 QEIx4_CCW | QEIx4_S0,
jocis 1:ac6b7b1bf6c5 24 QEIx4_CW | QEIx4_S1 | QEIx4_A | QEIx4_INC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 25 QEIx4_CCW | QEIx4_S0 | QEIx4_B,
jocis 1:ac6b7b1bf6c5 26 QEIx4_CCW | QEIx4_S3 | QEIx4_AB | QEIx4_DEC,
jocis 1:ac6b7b1bf6c5 27 // act state S1 in CCW direction
jocis 1:ac6b7b1bf6c5 28 QEIx4_CCW | QEIx4_S1,
jocis 1:ac6b7b1bf6c5 29 QEIx4_CCW | QEIx4_S1 | QEIx4_A,
jocis 1:ac6b7b1bf6c5 30 QEIx4_CCW | QEIx4_S0 | QEIx4_B | QEIx4_DEC,
jocis 1:ac6b7b1bf6c5 31 QEIx4_CW | QEIx4_S2 | QEIx4_AB | QEIx4_INC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 32 // act state S2 in CCW direction
jocis 1:ac6b7b1bf6c5 33 QEIx4_CCW | QEIx4_S1 | QEIx4_DEC,
jocis 1:ac6b7b1bf6c5 34 QEIx4_CCW | QEIx4_S2 | QEIx4_A,
jocis 1:ac6b7b1bf6c5 35 QEIx4_CW | QEIx4_S3 | QEIx4_B | QEIx4_INC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 36 QEIx4_CCW | QEIx4_S2 | QEIx4_AB,
jocis 1:ac6b7b1bf6c5 37 // act state S3 in CCW direction
jocis 1:ac6b7b1bf6c5 38 QEIx4_CW | QEIx4_S0 | QEIx4_INC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 39 QEIx4_CCW | QEIx4_S2 | QEIx4_A | QEIx4_DEC,
jocis 1:ac6b7b1bf6c5 40 QEIx4_CCW | QEIx4_S3 | QEIx4_B,
jocis 1:ac6b7b1bf6c5 41 QEIx4_CCW | QEIx4_S3 | QEIx4_AB,
jocis 1:ac6b7b1bf6c5 42
jocis 1:ac6b7b1bf6c5 43 // act state S0 in CW direction
jocis 1:ac6b7b1bf6c5 44 QEIx4_CW | QEIx4_S0,
jocis 1:ac6b7b1bf6c5 45 QEIx4_CW | QEIx4_S1 | QEIx4_A | QEIx4_INC,
jocis 1:ac6b7b1bf6c5 46 QEIx4_CW | QEIx4_S0 | QEIx4_B,
jocis 1:ac6b7b1bf6c5 47 QEIx4_CCW | QEIx4_S3 | QEIx4_AB | QEIx4_DEC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 48 // act state S1 in CW direction
jocis 1:ac6b7b1bf6c5 49 QEIx4_CW | QEIx4_S1,
jocis 1:ac6b7b1bf6c5 50 QEIx4_CW | QEIx4_S1 | QEIx4_A,
jocis 1:ac6b7b1bf6c5 51 QEIx4_CCW | QEIx4_S0 | QEIx4_B | QEIx4_DEC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 52 QEIx4_CW | QEIx4_S2 | QEIx4_AB | QEIx4_INC,
jocis 1:ac6b7b1bf6c5 53 // act state S2 in CW direction
jocis 1:ac6b7b1bf6c5 54 QEIx4_CCW | QEIx4_S1 | QEIx4_DEC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 55 QEIx4_CW | QEIx4_S2 | QEIx4_A,
jocis 1:ac6b7b1bf6c5 56 QEIx4_CW | QEIx4_S3 | QEIx4_B | QEIx4_INC,
jocis 1:ac6b7b1bf6c5 57 QEIx4_CW | QEIx4_S2 | QEIx4_AB,
jocis 1:ac6b7b1bf6c5 58 // act state S3 in CW direction
jocis 1:ac6b7b1bf6c5 59 QEIx4_CW | QEIx4_S0 | QEIx4_INC,
jocis 1:ac6b7b1bf6c5 60 QEIx4_CCW | QEIx4_S2 | QEIx4_A | QEIx4_DEC | QEIx4_DIR,
jocis 1:ac6b7b1bf6c5 61 QEIx4_CW | QEIx4_S3 | QEIx4_B,
jocis 1:ac6b7b1bf6c5 62 QEIx4_CW | QEIx4_S3 | QEIx4_AB
jocis 1:ac6b7b1bf6c5 63 };
jocis 1:ac6b7b1bf6c5 64
jocis 0:46b8d5680f66 65
jocis 0:46b8d5680f66 66 //#define DEB(x) printf (x)
jocis 0:46b8d5680f66 67 #define DEB(x)
jocis 0:46b8d5680f66 68
jocis 0:46b8d5680f66 69 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 70
jocis 1:ac6b7b1bf6c5 71 QEIx4::QEIx4 ( PinName pinA, PinName pinB, PinName pinI, EMODE eMode ) : _pinA(pinA), _pinB(pinB), _pinI(pinI)
jocis 0:46b8d5680f66 72 {
jocis 2:c0b87b11b9cd 73 // prepare input pins
jocis 1:ac6b7b1bf6c5 74 _pinA.mode(PullUp);
jocis 1:ac6b7b1bf6c5 75 _pinB.mode(PullUp);
jocis 1:ac6b7b1bf6c5 76 if(pinI!=NC)
jocis 1:ac6b7b1bf6c5 77 _pinI.mode(PullUp);
jocis 1:ac6b7b1bf6c5 78
jocis 2:c0b87b11b9cd 79 _eMode = eMode;
jocis 2:c0b87b11b9cd 80 _bIndexTrigger = false;
jocis 2:c0b87b11b9cd 81 _fPositionFactor = 1.0;
jocis 2:c0b87b11b9cd 82 _fSpeedFactor = 1.0;
jocis 2:c0b87b11b9cd 83 _counter = 0;
jocis 2:c0b87b11b9cd 84 _state = 0;
jocis 2:c0b87b11b9cd 85 _nSpeedLastTimer = 0;
jocis 2:c0b87b11b9cd 86 _nSpeedAvrTimeSum = 0;
jocis 2:c0b87b11b9cd 87 _nSpeedAvrTimeCount = -1;
jocis 2:c0b87b11b9cd 88 _fLastSpeed = 0;
jocis 2:c0b87b11b9cd 89 _nSpeedTimeoutMax = 10;
jocis 2:c0b87b11b9cd 90 _nSpeedTimeoutCount = 0;
jocis 2:c0b87b11b9cd 91
jocis 2:c0b87b11b9cd 92 // synchronize state machine
jocis 1:ac6b7b1bf6c5 93 if ( _eMode & IRQ ) {
jocis 1:ac6b7b1bf6c5 94 _pinA.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 95 _pinA.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 96 _pinB.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 97 _pinB.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 98 }
jocis 1:ac6b7b1bf6c5 99 if ( _eMode & IRQ_NO_JAMMING ) {
jocis 1:ac6b7b1bf6c5 100 ProcessISR ();
jocis 1:ac6b7b1bf6c5 101 _state += QEIx4_S2;
jocis 1:ac6b7b1bf6c5 102 }
jocis 0:46b8d5680f66 103 ProcessISR ();
jocis 0:46b8d5680f66 104 _counter = 0;
jocis 1:ac6b7b1bf6c5 105
jocis 2:c0b87b11b9cd 106 // prepare for speed measurement
jocis 1:ac6b7b1bf6c5 107 if ( _eMode & SPEED ) {
jocis 1:ac6b7b1bf6c5 108 _SpeedTimer.reset();
jocis 1:ac6b7b1bf6c5 109 _SpeedTimer.start();
jocis 1:ac6b7b1bf6c5 110 }
jocis 0:46b8d5680f66 111 }
jocis 0:46b8d5680f66 112
jocis 0:46b8d5680f66 113 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 114
jocis 0:46b8d5680f66 115 QEIx4::~QEIx4()
jocis 0:46b8d5680f66 116 {
jocis 0:46b8d5680f66 117 _pinA.rise ( NULL );
jocis 0:46b8d5680f66 118 _pinA.fall ( NULL );
jocis 0:46b8d5680f66 119 _pinB.rise ( NULL );
jocis 0:46b8d5680f66 120 _pinB.fall ( NULL );
jocis 0:46b8d5680f66 121 }
jocis 0:46b8d5680f66 122
jocis 0:46b8d5680f66 123 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 124
jocis 1:ac6b7b1bf6c5 125 float QEIx4::getSpeed()
jocis 1:ac6b7b1bf6c5 126 {
jocis 1:ac6b7b1bf6c5 127 float fSpeed;
jocis 1:ac6b7b1bf6c5 128 int avrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 129 int avrTimeCount = 0;
jocis 1:ac6b7b1bf6c5 130
jocis 1:ac6b7b1bf6c5 131 __disable_irq(); // Disable Interrupts for atomic copy
jocis 1:ac6b7b1bf6c5 132 avrTimeSum = _nSpeedAvrTimeSum;
jocis 1:ac6b7b1bf6c5 133 avrTimeCount = _nSpeedAvrTimeCount;
jocis 1:ac6b7b1bf6c5 134 _nSpeedAvrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 135 _nSpeedAvrTimeCount = 0;
jocis 1:ac6b7b1bf6c5 136 __enable_irq(); // Enable Interrupts
jocis 1:ac6b7b1bf6c5 137
jocis 1:ac6b7b1bf6c5 138 if ( avrTimeCount == 0 ) {
jocis 1:ac6b7b1bf6c5 139 if (_nSpeedTimeoutCount++ > _nSpeedTimeoutMax)
jocis 1:ac6b7b1bf6c5 140 _fLastSpeed *= 0.5f;
jocis 1:ac6b7b1bf6c5 141 fSpeed = _fLastSpeed;
jocis 1:ac6b7b1bf6c5 142 } else if ( avrTimeCount < 0 || avrTimeSum == 0 ) {
jocis 1:ac6b7b1bf6c5 143 fSpeed = 0;
jocis 1:ac6b7b1bf6c5 144 _nSpeedTimeoutCount = 0;
jocis 1:ac6b7b1bf6c5 145 } else {
jocis 1:ac6b7b1bf6c5 146 fSpeed = 1000000.0f * _fSpeedFactor / ( (float)avrTimeSum / (float)avrTimeCount );
jocis 1:ac6b7b1bf6c5 147 _nSpeedTimeoutCount = 0;
jocis 1:ac6b7b1bf6c5 148 }
jocis 1:ac6b7b1bf6c5 149 _fLastSpeed = fSpeed;
jocis 1:ac6b7b1bf6c5 150
jocis 1:ac6b7b1bf6c5 151 return fSpeed;
jocis 1:ac6b7b1bf6c5 152 }
jocis 1:ac6b7b1bf6c5 153
jocis 1:ac6b7b1bf6c5 154 ///////////////////////////////////////////////////////////////////////////////
jocis 1:ac6b7b1bf6c5 155
jocis 1:ac6b7b1bf6c5 156 void QEIx4::ProcessISR ( void )
jocis 0:46b8d5680f66 157 {
jocis 2:c0b87b11b9cd 158 int nLoopCounter=10;
jocis 1:ac6b7b1bf6c5 159 int pinA, pinB;
jocis 1:ac6b7b1bf6c5 160
jocis 1:ac6b7b1bf6c5 161 do {
jocis 1:ac6b7b1bf6c5 162 pinA = _pinA;
jocis 1:ac6b7b1bf6c5 163 pinB = _pinB;
jocis 1:ac6b7b1bf6c5 164
jocis 1:ac6b7b1bf6c5 165 DEB (".");
jocis 1:ac6b7b1bf6c5 166 _state &= QEIx4_MASK;
jocis 1:ac6b7b1bf6c5 167 if ( pinA ) _state |= QEIx4_A;
jocis 1:ac6b7b1bf6c5 168 if ( pinB ) _state |= QEIx4_B;
jocis 1:ac6b7b1bf6c5 169
jocis 1:ac6b7b1bf6c5 170 _state = _modeLUT[_state]; // magic is done by lookup-table
jocis 1:ac6b7b1bf6c5 171
jocis 1:ac6b7b1bf6c5 172 if ( _state & QEIx4_CHG ) { // is any change?
jocis 1:ac6b7b1bf6c5 173 bool bCounterChange = false;
jocis 1:ac6b7b1bf6c5 174
jocis 1:ac6b7b1bf6c5 175 if ( _state & QEIx4_INC ) { // is moved foreward?
jocis 1:ac6b7b1bf6c5 176 _counter++;
jocis 1:ac6b7b1bf6c5 177 bCounterChange = true;
jocis 1:ac6b7b1bf6c5 178 }
jocis 1:ac6b7b1bf6c5 179 if ( _state & QEIx4_DEC ) { // is moved backward?
jocis 1:ac6b7b1bf6c5 180 _counter--;
jocis 1:ac6b7b1bf6c5 181 bCounterChange = true;
jocis 1:ac6b7b1bf6c5 182 }
jocis 1:ac6b7b1bf6c5 183
jocis 2:c0b87b11b9cd 184 if ( _eMode & IRQ_NO_JAMMING ) { // need reconfiguration of interrupt edges?
jocis 1:ac6b7b1bf6c5 185 switch ( _state & QEIx4_STATE ) {
jocis 1:ac6b7b1bf6c5 186 case QEIx4_S0:
jocis 1:ac6b7b1bf6c5 187 _pinB.rise ( NULL );
jocis 1:ac6b7b1bf6c5 188 _pinB.fall ( NULL );
jocis 1:ac6b7b1bf6c5 189 _pinA.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 190 DEB ("S0");
jocis 1:ac6b7b1bf6c5 191 break;
jocis 1:ac6b7b1bf6c5 192 case QEIx4_S1:
jocis 1:ac6b7b1bf6c5 193 _pinA.rise ( NULL );
jocis 1:ac6b7b1bf6c5 194 _pinA.fall ( NULL );
jocis 1:ac6b7b1bf6c5 195 _pinB.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 196 DEB ("S1");
jocis 1:ac6b7b1bf6c5 197 break;
jocis 1:ac6b7b1bf6c5 198 case QEIx4_S2:
jocis 1:ac6b7b1bf6c5 199 _pinB.rise ( NULL );
jocis 1:ac6b7b1bf6c5 200 _pinB.fall ( NULL );
jocis 1:ac6b7b1bf6c5 201 _pinA.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 202 DEB ("S2");
jocis 1:ac6b7b1bf6c5 203 break;
jocis 1:ac6b7b1bf6c5 204 case QEIx4_S3:
jocis 1:ac6b7b1bf6c5 205 _pinA.rise ( NULL );
jocis 1:ac6b7b1bf6c5 206 _pinA.fall ( NULL );
jocis 1:ac6b7b1bf6c5 207 _pinB.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 208 DEB ("S3");
jocis 1:ac6b7b1bf6c5 209 break;
jocis 1:ac6b7b1bf6c5 210 }
jocis 1:ac6b7b1bf6c5 211 }
jocis 1:ac6b7b1bf6c5 212
jocis 1:ac6b7b1bf6c5 213 if ( _eMode & SPEED ) {
jocis 1:ac6b7b1bf6c5 214 unsigned int act = _SpeedTimer.read_us();
jocis 1:ac6b7b1bf6c5 215 unsigned int diff = act - _nSpeedLastTimer; // Note: overflow is handled correctly
jocis 1:ac6b7b1bf6c5 216 _nSpeedLastTimer = act;
jocis 1:ac6b7b1bf6c5 217
jocis 1:ac6b7b1bf6c5 218 if ( _nSpeedAvrTimeCount < 0 ) { // ignore first pulse to synchronize timer (maybe timer overflow)
jocis 1:ac6b7b1bf6c5 219 _nSpeedAvrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 220 _nSpeedAvrTimeCount = 0;
jocis 1:ac6b7b1bf6c5 221 } else {
jocis 1:ac6b7b1bf6c5 222 if ( _state & QEIx4_CW ) // is moving foreward?
jocis 1:ac6b7b1bf6c5 223 _nSpeedAvrTimeSum += diff;
jocis 1:ac6b7b1bf6c5 224 else
jocis 1:ac6b7b1bf6c5 225 _nSpeedAvrTimeSum -= diff;
jocis 1:ac6b7b1bf6c5 226 _nSpeedAvrTimeCount++;
jocis 1:ac6b7b1bf6c5 227 }
jocis 1:ac6b7b1bf6c5 228 }
jocis 1:ac6b7b1bf6c5 229
jocis 2:c0b87b11b9cd 230 if ( _bIndexTrigger && bCounterChange && _pinI != NC && _pinI == 1 ) { // is index pin triggered?
jocis 2:c0b87b11b9cd 231 _bIndexTrigger = false;
jocis 2:c0b87b11b9cd 232 fPointerIndexTrigger.call(_counter);
jocis 2:c0b87b11b9cd 233 }
jocis 2:c0b87b11b9cd 234
jocis 1:ac6b7b1bf6c5 235 if ( bCounterChange ) { // has counter changed?
jocis 1:ac6b7b1bf6c5 236 fPointerCounterChange.call(_counter);
jocis 1:ac6b7b1bf6c5 237 if ( _state & QEIx4_DIR )
jocis 1:ac6b7b1bf6c5 238 fPointerDirectionChange.call(_counter);
jocis 1:ac6b7b1bf6c5 239 }
jocis 1:ac6b7b1bf6c5 240
jocis 0:46b8d5680f66 241 }
jocis 2:c0b87b11b9cd 242 } while (nLoopCounter-->0 && ( pinA != _pinA || pinB != _pinB)); // loop till stable input pins
jocis 0:46b8d5680f66 243 }
jocis 0:46b8d5680f66 244
jocis 0:46b8d5680f66 245 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 246 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 247 ///////////////////////////////////////////////////////////////////////////////