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@1:ac6b7b1bf6c5, 2014-09-30 (annotated)

Committer:
jocis
Date:
Tue Sep 30 12:34:07 2014 +0000
Revision:
1:ac6b7b1bf6c5
Parent:
0:46b8d5680f66
Child:
2:c0b87b11b9cd
Added speed measurement; improved state machine and irq handling

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 1:ac6b7b1bf6c5 73 _eMode = eMode;
jocis 0:46b8d5680f66 74 _bZeroOnIndex = false;
jocis 1:ac6b7b1bf6c5 75 _fPositionFactor = 1.0;
jocis 1:ac6b7b1bf6c5 76
jocis 0:46b8d5680f66 77 // synchronize state machine
jocis 0:46b8d5680f66 78 _counter = 0;
jocis 1:ac6b7b1bf6c5 79 _state = 0;
jocis 1:ac6b7b1bf6c5 80
jocis 1:ac6b7b1bf6c5 81 _pinA.mode(PullUp);
jocis 1:ac6b7b1bf6c5 82 _pinB.mode(PullUp);
jocis 1:ac6b7b1bf6c5 83 if(pinI!=NC)
jocis 1:ac6b7b1bf6c5 84 _pinI.mode(PullUp);
jocis 1:ac6b7b1bf6c5 85
jocis 1:ac6b7b1bf6c5 86 if ( _eMode & IRQ ) {
jocis 1:ac6b7b1bf6c5 87 _pinA.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 88 _pinA.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 89 _pinB.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 90 _pinB.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 91 }
jocis 1:ac6b7b1bf6c5 92 if ( _eMode & IRQ_NO_JAMMING ) {
jocis 1:ac6b7b1bf6c5 93 ProcessISR ();
jocis 1:ac6b7b1bf6c5 94 _state += QEIx4_S2;
jocis 1:ac6b7b1bf6c5 95 }
jocis 0:46b8d5680f66 96 ProcessISR ();
jocis 0:46b8d5680f66 97 _counter = 0;
jocis 1:ac6b7b1bf6c5 98
jocis 1:ac6b7b1bf6c5 99 if ( _eMode & SPEED ) {
jocis 1:ac6b7b1bf6c5 100 _SpeedTimer.reset();
jocis 1:ac6b7b1bf6c5 101 _SpeedTimer.start();
jocis 1:ac6b7b1bf6c5 102 }
jocis 1:ac6b7b1bf6c5 103
jocis 1:ac6b7b1bf6c5 104 _nSpeedLastTimer = 0;
jocis 1:ac6b7b1bf6c5 105 _nSpeedAvrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 106 _nSpeedAvrTimeCount = -1;
jocis 1:ac6b7b1bf6c5 107 _fSpeedFactor = 1.0f;
jocis 1:ac6b7b1bf6c5 108 _fLastSpeed = 0;
jocis 1:ac6b7b1bf6c5 109
jocis 1:ac6b7b1bf6c5 110 _nSpeedTimeoutMax = 10;
jocis 1:ac6b7b1bf6c5 111 _nSpeedTimeoutCount = 0;
jocis 1:ac6b7b1bf6c5 112
jocis 0:46b8d5680f66 113 }
jocis 0:46b8d5680f66 114
jocis 0:46b8d5680f66 115 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 116
jocis 0:46b8d5680f66 117 QEIx4::~QEIx4()
jocis 0:46b8d5680f66 118 {
jocis 0:46b8d5680f66 119 _pinA.rise ( NULL );
jocis 0:46b8d5680f66 120 _pinA.fall ( NULL );
jocis 0:46b8d5680f66 121 _pinB.rise ( NULL );
jocis 0:46b8d5680f66 122 _pinB.fall ( NULL );
jocis 0:46b8d5680f66 123 }
jocis 0:46b8d5680f66 124
jocis 0:46b8d5680f66 125 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 126
jocis 1:ac6b7b1bf6c5 127 float QEIx4::getSpeed()
jocis 1:ac6b7b1bf6c5 128 {
jocis 1:ac6b7b1bf6c5 129 float fSpeed;
jocis 1:ac6b7b1bf6c5 130 int avrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 131 int avrTimeCount = 0;
jocis 1:ac6b7b1bf6c5 132
jocis 1:ac6b7b1bf6c5 133 __disable_irq(); // Disable Interrupts for atomic copy
jocis 1:ac6b7b1bf6c5 134 avrTimeSum = _nSpeedAvrTimeSum;
jocis 1:ac6b7b1bf6c5 135 avrTimeCount = _nSpeedAvrTimeCount;
jocis 1:ac6b7b1bf6c5 136 _nSpeedAvrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 137 _nSpeedAvrTimeCount = 0;
jocis 1:ac6b7b1bf6c5 138 __enable_irq(); // Enable Interrupts
jocis 1:ac6b7b1bf6c5 139
jocis 1:ac6b7b1bf6c5 140 if ( avrTimeCount == 0 ) {
jocis 1:ac6b7b1bf6c5 141 if (_nSpeedTimeoutCount++ > _nSpeedTimeoutMax)
jocis 1:ac6b7b1bf6c5 142 _fLastSpeed *= 0.5f;
jocis 1:ac6b7b1bf6c5 143 fSpeed = _fLastSpeed;
jocis 1:ac6b7b1bf6c5 144 } else if ( avrTimeCount < 0 || avrTimeSum == 0 ) {
jocis 1:ac6b7b1bf6c5 145 fSpeed = 0;
jocis 1:ac6b7b1bf6c5 146 _nSpeedTimeoutCount = 0;
jocis 1:ac6b7b1bf6c5 147 } else {
jocis 1:ac6b7b1bf6c5 148 fSpeed = 1000000.0f * _fSpeedFactor / ( (float)avrTimeSum / (float)avrTimeCount );
jocis 1:ac6b7b1bf6c5 149 _nSpeedTimeoutCount = 0;
jocis 1:ac6b7b1bf6c5 150 }
jocis 1:ac6b7b1bf6c5 151 _fLastSpeed = fSpeed;
jocis 1:ac6b7b1bf6c5 152
jocis 1:ac6b7b1bf6c5 153 return fSpeed;
jocis 1:ac6b7b1bf6c5 154 }
jocis 1:ac6b7b1bf6c5 155
jocis 1:ac6b7b1bf6c5 156 ///////////////////////////////////////////////////////////////////////////////
jocis 1:ac6b7b1bf6c5 157
jocis 1:ac6b7b1bf6c5 158 void QEIx4::ProcessISR ( void )
jocis 0:46b8d5680f66 159 {
jocis 1:ac6b7b1bf6c5 160 int pinA, pinB;
jocis 1:ac6b7b1bf6c5 161
jocis 1:ac6b7b1bf6c5 162 do {
jocis 1:ac6b7b1bf6c5 163 pinA = _pinA;
jocis 1:ac6b7b1bf6c5 164 pinB = _pinB;
jocis 1:ac6b7b1bf6c5 165
jocis 1:ac6b7b1bf6c5 166 DEB (".");
jocis 1:ac6b7b1bf6c5 167 _state &= QEIx4_MASK;
jocis 1:ac6b7b1bf6c5 168 if ( pinA ) _state |= QEIx4_A;
jocis 1:ac6b7b1bf6c5 169 if ( pinB ) _state |= QEIx4_B;
jocis 1:ac6b7b1bf6c5 170
jocis 1:ac6b7b1bf6c5 171 _state = _modeLUT[_state]; // magic is done by lookup-table
jocis 1:ac6b7b1bf6c5 172
jocis 1:ac6b7b1bf6c5 173 if ( _state & QEIx4_CHG ) { // is any change?
jocis 1:ac6b7b1bf6c5 174 bool bCounterChange = false;
jocis 1:ac6b7b1bf6c5 175
jocis 1:ac6b7b1bf6c5 176 if ( _state & QEIx4_INC ) { // is moved foreward?
jocis 1:ac6b7b1bf6c5 177 _counter++;
jocis 1:ac6b7b1bf6c5 178 bCounterChange = true;
jocis 1:ac6b7b1bf6c5 179 }
jocis 1:ac6b7b1bf6c5 180 if ( _state & QEIx4_DEC ) { // is moved backward?
jocis 1:ac6b7b1bf6c5 181 _counter--;
jocis 1:ac6b7b1bf6c5 182 bCounterChange = true;
jocis 1:ac6b7b1bf6c5 183 }
jocis 1:ac6b7b1bf6c5 184 if ( _bZeroOnIndex && _pinI != NC && _pinI == 1 ) { // is index pin triggered?
jocis 1:ac6b7b1bf6c5 185 _counter = 0;
jocis 1:ac6b7b1bf6c5 186 _bZeroOnIndex = false;
jocis 1:ac6b7b1bf6c5 187 CounterZero ();
jocis 1:ac6b7b1bf6c5 188 bCounterChange = true;
jocis 1:ac6b7b1bf6c5 189 }
jocis 1:ac6b7b1bf6c5 190
jocis 1:ac6b7b1bf6c5 191 if ( _eMode & IRQ_NO_JAMMING ) { // need reconfiguration od interrupt edges?
jocis 1:ac6b7b1bf6c5 192 switch ( _state & QEIx4_STATE ) {
jocis 1:ac6b7b1bf6c5 193 case QEIx4_S0:
jocis 1:ac6b7b1bf6c5 194 _pinB.rise ( NULL );
jocis 1:ac6b7b1bf6c5 195 _pinB.fall ( NULL );
jocis 1:ac6b7b1bf6c5 196 _pinA.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 197 DEB ("S0");
jocis 1:ac6b7b1bf6c5 198 break;
jocis 1:ac6b7b1bf6c5 199 case QEIx4_S1:
jocis 1:ac6b7b1bf6c5 200 _pinA.rise ( NULL );
jocis 1:ac6b7b1bf6c5 201 _pinA.fall ( NULL );
jocis 1:ac6b7b1bf6c5 202 _pinB.rise ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 203 DEB ("S1");
jocis 1:ac6b7b1bf6c5 204 break;
jocis 1:ac6b7b1bf6c5 205 case QEIx4_S2:
jocis 1:ac6b7b1bf6c5 206 _pinB.rise ( NULL );
jocis 1:ac6b7b1bf6c5 207 _pinB.fall ( NULL );
jocis 1:ac6b7b1bf6c5 208 _pinA.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 209 DEB ("S2");
jocis 1:ac6b7b1bf6c5 210 break;
jocis 1:ac6b7b1bf6c5 211 case QEIx4_S3:
jocis 1:ac6b7b1bf6c5 212 _pinA.rise ( NULL );
jocis 1:ac6b7b1bf6c5 213 _pinA.fall ( NULL );
jocis 1:ac6b7b1bf6c5 214 _pinB.fall ( this, &QEIx4::ProcessISR );
jocis 1:ac6b7b1bf6c5 215 DEB ("S3");
jocis 1:ac6b7b1bf6c5 216 break;
jocis 1:ac6b7b1bf6c5 217 }
jocis 1:ac6b7b1bf6c5 218 }
jocis 1:ac6b7b1bf6c5 219
jocis 1:ac6b7b1bf6c5 220 if ( _eMode & SPEED ) {
jocis 1:ac6b7b1bf6c5 221 unsigned int act = _SpeedTimer.read_us();
jocis 1:ac6b7b1bf6c5 222 unsigned int diff = act - _nSpeedLastTimer; // Note: overflow is handled correctly
jocis 1:ac6b7b1bf6c5 223 _nSpeedLastTimer = act;
jocis 1:ac6b7b1bf6c5 224
jocis 1:ac6b7b1bf6c5 225 if ( _nSpeedAvrTimeCount < 0 ) { // ignore first pulse to synchronize timer (maybe timer overflow)
jocis 1:ac6b7b1bf6c5 226 _nSpeedAvrTimeSum = 0;
jocis 1:ac6b7b1bf6c5 227 _nSpeedAvrTimeCount = 0;
jocis 1:ac6b7b1bf6c5 228 } else {
jocis 1:ac6b7b1bf6c5 229 if ( _state & QEIx4_CW ) // is moving foreward?
jocis 1:ac6b7b1bf6c5 230 _nSpeedAvrTimeSum += diff;
jocis 1:ac6b7b1bf6c5 231 else
jocis 1:ac6b7b1bf6c5 232 _nSpeedAvrTimeSum -= diff;
jocis 1:ac6b7b1bf6c5 233 _nSpeedAvrTimeCount++;
jocis 1:ac6b7b1bf6c5 234 }
jocis 1:ac6b7b1bf6c5 235 }
jocis 1:ac6b7b1bf6c5 236
jocis 1:ac6b7b1bf6c5 237 if ( bCounterChange ) { // has counter changed?
jocis 1:ac6b7b1bf6c5 238 CounterChange ();
jocis 1:ac6b7b1bf6c5 239 fPointerCounterChange.call(_counter);
jocis 1:ac6b7b1bf6c5 240 if ( _state & QEIx4_DIR )
jocis 1:ac6b7b1bf6c5 241 fPointerDirectionChange.call(_counter);
jocis 1:ac6b7b1bf6c5 242 }
jocis 1:ac6b7b1bf6c5 243
jocis 0:46b8d5680f66 244 }
jocis 1:ac6b7b1bf6c5 245 } while ( pinA != _pinA || pinB != _pinB); // loop till stable input pins
jocis 0:46b8d5680f66 246 }
jocis 0:46b8d5680f66 247
jocis 1:ac6b7b1bf6c5 248 //////////////////////////////////////////////////////////////////////////////////
jocis 1:ac6b7b1bf6c5 249
jocis 0:46b8d5680f66 250 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 251 ///////////////////////////////////////////////////////////////////////////////
jocis 0:46b8d5680f66 252 ///////////////////////////////////////////////////////////////////////////////