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.h@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 1:ac6b7b1bf6c5 1 /*
jocis 0:46b8d5680f66 2 * @author Jochen Krapf
jocis 1:ac6b7b1bf6c5 3 * parts by Andy Kirkham
jocis 0:46b8d5680f66 4 *
jocis 0:46b8d5680f66 5 * @section LICENSE
jocis 0:46b8d5680f66 6 *
jocis 1:ac6b7b1bf6c5 7 * Copyright (c) 2014 Jochen Krapf, MIT License
jocis 0:46b8d5680f66 8 *
jocis 0:46b8d5680f66 9 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
jocis 0:46b8d5680f66 10 * and associated documentation files (the "Software"), to deal in the Software without restriction,
jocis 0:46b8d5680f66 11 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
jocis 0:46b8d5680f66 12 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
jocis 0:46b8d5680f66 13 * furnished to do so, subject to the following conditions:
jocis 0:46b8d5680f66 14 *
jocis 0:46b8d5680f66 15 * The above copyright notice and this permission notice shall be included in all copies or
jocis 0:46b8d5680f66 16 * substantial portions of the Software.
jocis 0:46b8d5680f66 17 *
jocis 0:46b8d5680f66 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
jocis 0:46b8d5680f66 19 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
jocis 0:46b8d5680f66 20 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
jocis 0:46b8d5680f66 21 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jocis 0:46b8d5680f66 22 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
jocis 0:46b8d5680f66 23 *
jocis 0:46b8d5680f66 24 * @section DESCRIPTION
jocis 1:ac6b7b1bf6c5 25 * mbed QEI (quadrature encoder interface) library, for decoding AB signals from a rotary encoder
jocis 0:46b8d5680f66 26 *
jocis 0:46b8d5680f66 27 * Use cases:
jocis 0:46b8d5680f66 28 * - Rotary encoder in closed loop motor regulation
jocis 0:46b8d5680f66 29 * - Hand wheel
jocis 0:46b8d5680f66 30 * - Input device for motion control (MoCo)
jocis 0:46b8d5680f66 31 */
jocis 0:46b8d5680f66 32
jocis 0:46b8d5680f66 33 #include "mbed.h"
jocis 1:ac6b7b1bf6c5 34 #include "FPointer_vi.h"
jocis 0:46b8d5680f66 35
jocis 1:ac6b7b1bf6c5 36 /** === Quadrature Encoder Interface. ===
jocis 1:ac6b7b1bf6c5 37 *
jocis 1:ac6b7b1bf6c5 38 * A class to decode pulses on a AB rotary encoder. It uses all 4 edges of the AB signals to increase resolution 4 times of cycles per rotation (CPR) (e.g. an encoder with 500 CPR get 2000 counts per rotation)
jocis 1:ac6b7b1bf6c5 39 *
jocis 1:ac6b7b1bf6c5 40 * In opposite to most common QEI implementation this is resistant to jitter on AB signals and motor vibration. When using interrupts only (IRQ_NO_JAMMING-mode) the needed edge and pin is activated to prevent jamming CPU time with unnecessary interrupts.
jocis 0:46b8d5680f66 41 *
jocis 1:ac6b7b1bf6c5 42 * It can also be used in polling mode i.g. in idle routines if interrupts are not desired.
jocis 1:ac6b7b1bf6c5 43 *
jocis 1:ac6b7b1bf6c5 44 * The internal state machine is based on a look up table (LUT) to minimize interrupt retention time and get all necessary flags at once.
jocis 0:46b8d5680f66 45 *
jocis 1:ac6b7b1bf6c5 46 * 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.
jocis 1:ac6b7b1bf6c5 47 *
jocis 1:ac6b7b1bf6c5 48 * The library is designed to support closed loop speed- and motion-controller for also slow and smooth motions like movie camera motion control.
jocis 0:46b8d5680f66 49 *
jocis 0:46b8d5680f66 50 * Example:
jocis 0:46b8d5680f66 51 *
jocis 0:46b8d5680f66 52 * @code
jocis 0:46b8d5680f66 53 #include "mbed.h"
jocis 0:46b8d5680f66 54 #include "QEIx4.h"
jocis 0:46b8d5680f66 55
jocis 0:46b8d5680f66 56 DigitalOut myled(LED1);
jocis 0:46b8d5680f66 57 Timer t;
jocis 0:46b8d5680f66 58
jocis 0:46b8d5680f66 59 // ports for nxp LPC 1768
jocis 0:46b8d5680f66 60 QEIx4 qei1(p30, p29, p28); // QEI with index signal for zeroing
jocis 0:46b8d5680f66 61 QEIx4 qei2(p27, p26, NC); // QEI only with AB signals
jocis 1:ac6b7b1bf6c5 62 QEIx4 qei3(p25, p24, NC, QEIx4::POLLING); // QEI without interrups for polling mode
jocis 0:46b8d5680f66 63
jocis 0:46b8d5680f66 64 int main() {
jocis 0:46b8d5680f66 65 t.start();
jocis 0:46b8d5680f66 66
jocis 0:46b8d5680f66 67 qei1.SetZeroOnIndex(true); // Set the flag to zero counter on next index signal rises
jocis 0:46b8d5680f66 68
jocis 1:ac6b7b1bf6c5 69 while(1)
jocis 0:46b8d5680f66 70 {
jocis 0:46b8d5680f66 71 qei3.poll(); // poll manually without interrupt - sampling in this loop with about 2kHz
jocis 1:ac6b7b1bf6c5 72
jocis 0:46b8d5680f66 73 if ( t.read_ms() > 500 ) // every half second
jocis 0:46b8d5680f66 74 {
jocis 0:46b8d5680f66 75 t.reset();
jocis 0:46b8d5680f66 76 t.start();
jocis 0:46b8d5680f66 77 myled = !myled;
jocis 1:ac6b7b1bf6c5 78
jocis 0:46b8d5680f66 79 printf ( "\r\n%6d %6d %6d", (int)qei1, (int)qei2, (int)qei3 ); // print counter values
jocis 0:46b8d5680f66 80 }
jocis 0:46b8d5680f66 81
jocis 1:ac6b7b1bf6c5 82 wait_us(20); // for about 50kHz
jocis 0:46b8d5680f66 83 }
jocis 0:46b8d5680f66 84 }
jocis 0:46b8d5680f66 85 * @endcode
jocis 0:46b8d5680f66 86 */
jocis 1:ac6b7b1bf6c5 87 class QEIx4
jocis 1:ac6b7b1bf6c5 88 {
jocis 0:46b8d5680f66 89 public:
jocis 0:46b8d5680f66 90
jocis 1:ac6b7b1bf6c5 91 typedef enum EMODE {
jocis 1:ac6b7b1bf6c5 92 POLLING = 0,
jocis 1:ac6b7b1bf6c5 93 IRQ = 1,
jocis 1:ac6b7b1bf6c5 94 IRQ_NO_JAMMING = 2,
jocis 1:ac6b7b1bf6c5 95 SPEED = 4,
jocis 1:ac6b7b1bf6c5 96 } EMODE;
jocis 1:ac6b7b1bf6c5 97
jocis 0:46b8d5680f66 98 /** constructor of QEIx4 object
jocis 0:46b8d5680f66 99 *
jocis 0:46b8d5680f66 100 * @param pinA Pin number of input/interrupt pin for encoder line A. All port pins are possible except p19 and p20
jocis 0:46b8d5680f66 101 * @param pinB Pin number of input/interrupt pin for encoder line B. All port pins are possible except p19 and p20
jocis 0:46b8d5680f66 102 * @param pinI Pin number of input pin for optional encoder index or reference switch.
jocis 1:ac6b7b1bf6c5 103 * @param eMode Flag to use interrups to detect changes on line A and B. For none interrupt use mode POLLING and call the function poll() frequently. For optional speed calculation the mode SPEED can be ored
jocis 0:46b8d5680f66 104 */
jocis 1:ac6b7b1bf6c5 105 QEIx4 ( PinName pinA, PinName pinB, PinName pinI=NC, EMODE eMode=IRQ );
jocis 1:ac6b7b1bf6c5 106
jocis 0:46b8d5680f66 107 /** destructor of QEIx4 object
jocis 0:46b8d5680f66 108 */
jocis 0:46b8d5680f66 109 ~QEIx4();
jocis 0:46b8d5680f66 110
jocis 0:46b8d5680f66 111 /** Gets the actual counter value.
jocis 0:46b8d5680f66 112 *
jocis 0:46b8d5680f66 113 * @return Actual counter value
jocis 0:46b8d5680f66 114 */
jocis 1:ac6b7b1bf6c5 115 int read() {
jocis 1:ac6b7b1bf6c5 116 return _counter;
jocis 1:ac6b7b1bf6c5 117 }
jocis 0:46b8d5680f66 118
jocis 0:46b8d5680f66 119 /** Gets the actual counter value as int operator.
jocis 0:46b8d5680f66 120 *
jocis 0:46b8d5680f66 121 * @return Actual counter value as int operator
jocis 0:46b8d5680f66 122 */
jocis 1:ac6b7b1bf6c5 123 operator int () { // int-Operator
jocis 1:ac6b7b1bf6c5 124 return _counter;
jocis 1:ac6b7b1bf6c5 125 }
jocis 1:ac6b7b1bf6c5 126
jocis 0:46b8d5680f66 127 /** Sets the counter value at actual encoder position to given value.
jocis 0:46b8d5680f66 128 *
jocis 0:46b8d5680f66 129 * @param Counter value
jocis 0:46b8d5680f66 130 */
jocis 1:ac6b7b1bf6c5 131 void write ( int counter ) {
jocis 1:ac6b7b1bf6c5 132 _counter = counter;
jocis 1:ac6b7b1bf6c5 133 }
jocis 1:ac6b7b1bf6c5 134
jocis 0:46b8d5680f66 135 /** Sets the counter value at actual encoder position to given value as assign operator.
jocis 0:46b8d5680f66 136 *
jocis 0:46b8d5680f66 137 * @param Counter value
jocis 0:46b8d5680f66 138 */
jocis 1:ac6b7b1bf6c5 139 int operator= ( int counter ) { // Assign-Operator
jocis 1:ac6b7b1bf6c5 140 write(counter);
jocis 1:ac6b7b1bf6c5 141 return counter;
jocis 1:ac6b7b1bf6c5 142 }
jocis 1:ac6b7b1bf6c5 143
jocis 0:46b8d5680f66 144 /** Polls the state machine manually and updates the counter value.
jocis 0:46b8d5680f66 145 */
jocis 1:ac6b7b1bf6c5 146 void poll () {
jocis 1:ac6b7b1bf6c5 147 ProcessISR();
jocis 1:ac6b7b1bf6c5 148 }
jocis 1:ac6b7b1bf6c5 149
jocis 0:46b8d5680f66 150 /** Sets the flag for zeroing on next high on index pin while AB lines triggers next counting. The trigger forces the counter set to zero
jocis 0:46b8d5680f66 151 *
jocis 0:46b8d5680f66 152 * @param Flag for triggering. Is reset on next zeroing
jocis 0:46b8d5680f66 153 */
jocis 1:ac6b7b1bf6c5 154 void setZeroOnIndex ( bool bZeroOnIndex ) {
jocis 1:ac6b7b1bf6c5 155 _bZeroOnIndex = bZeroOnIndex;
jocis 1:ac6b7b1bf6c5 156 }
jocis 0:46b8d5680f66 157
jocis 0:46b8d5680f66 158 /** Callback in derived classes to act on counter change
jocis 0:46b8d5680f66 159 */
jocis 0:46b8d5680f66 160 virtual void CounterChange ( void ) {};
jocis 0:46b8d5680f66 161
jocis 0:46b8d5680f66 162 /** Callback in derived classes to act on zeroing trigger
jocis 0:46b8d5680f66 163 */
jocis 0:46b8d5680f66 164 virtual void CounterZero ( void ) {};
jocis 1:ac6b7b1bf6c5 165
jocis 1:ac6b7b1bf6c5 166 /** attach - Overloaded attachment function.
jocis 1:ac6b7b1bf6c5 167 *
jocis 1:ac6b7b1bf6c5 168 * Attach a C type function pointer as the callback.
jocis 1:ac6b7b1bf6c5 169 *
jocis 1:ac6b7b1bf6c5 170 * Note, the callback function prototype must be:-
jocis 1:ac6b7b1bf6c5 171 * @code
jocis 1:ac6b7b1bf6c5 172 * void myCallbackFunction(int);
jocis 1:ac6b7b1bf6c5 173 * @endcode
jocis 1:ac6b7b1bf6c5 174 * @param A C function pointer to call.
jocis 1:ac6b7b1bf6c5 175 */
jocis 1:ac6b7b1bf6c5 176 void attachCounterChange(void (*function)(int) = 0) {
jocis 1:ac6b7b1bf6c5 177 fPointerCounterChange.attach (function);
jocis 1:ac6b7b1bf6c5 178 }
jocis 1:ac6b7b1bf6c5 179
jocis 1:ac6b7b1bf6c5 180 /** attachCounterChange - Overloaded attachment function.
jocis 1:ac6b7b1bf6c5 181 *
jocis 1:ac6b7b1bf6c5 182 * Attach a C++ type object/method pointer as the callback.
jocis 1:ac6b7b1bf6c5 183 *
jocis 1:ac6b7b1bf6c5 184 * Note, the callback method prototype must be:-
jocis 1:ac6b7b1bf6c5 185 * @code
jocis 1:ac6b7b1bf6c5 186 * public:
jocis 1:ac6b7b1bf6c5 187 * static void myCallbackFunction(int);
jocis 1:ac6b7b1bf6c5 188 * @endcode
jocis 1:ac6b7b1bf6c5 189 * @param A C++ object pointer.
jocis 1:ac6b7b1bf6c5 190 * @param A C++ method within the object to call.
jocis 1:ac6b7b1bf6c5 191 */
jocis 1:ac6b7b1bf6c5 192 template<class T>
jocis 1:ac6b7b1bf6c5 193 void attachCounterChange(T* item, void (T::*method)(int)) {
jocis 1:ac6b7b1bf6c5 194 fPointerCounterChange.attach( item, method);
jocis 1:ac6b7b1bf6c5 195 }
jocis 1:ac6b7b1bf6c5 196
jocis 1:ac6b7b1bf6c5 197 /** attachDirectionChange - Overloaded attachment function.
jocis 1:ac6b7b1bf6c5 198 *
jocis 1:ac6b7b1bf6c5 199 * Attach a C type function pointer as the callback.
jocis 1:ac6b7b1bf6c5 200 *
jocis 1:ac6b7b1bf6c5 201 * Note, the callback function prototype must be:-
jocis 1:ac6b7b1bf6c5 202 * @code
jocis 1:ac6b7b1bf6c5 203 * void myCallbackFunction(int);
jocis 1:ac6b7b1bf6c5 204 * @endcode
jocis 1:ac6b7b1bf6c5 205 * @param A C function pointer to call.
jocis 1:ac6b7b1bf6c5 206 */
jocis 1:ac6b7b1bf6c5 207 void attachDirectionChange(void (*function)(int) = 0) {
jocis 1:ac6b7b1bf6c5 208 fPointerDirectionChange.attach (function);
jocis 1:ac6b7b1bf6c5 209 }
jocis 1:ac6b7b1bf6c5 210
jocis 1:ac6b7b1bf6c5 211 /** attachDirectionChange - Overloaded attachment function.
jocis 1:ac6b7b1bf6c5 212 *
jocis 1:ac6b7b1bf6c5 213 * Attach a C++ type object/method pointer as the callback.
jocis 1:ac6b7b1bf6c5 214 *
jocis 1:ac6b7b1bf6c5 215 * Note, the callback method prototype must be:-
jocis 1:ac6b7b1bf6c5 216 * @code
jocis 1:ac6b7b1bf6c5 217 * public:
jocis 1:ac6b7b1bf6c5 218 * static void myCallbackFunction(int);
jocis 1:ac6b7b1bf6c5 219 * @endcode
jocis 1:ac6b7b1bf6c5 220 * @param A C++ object pointer.
jocis 1:ac6b7b1bf6c5 221 * @param A C++ method within the object to call.
jocis 1:ac6b7b1bf6c5 222 */
jocis 1:ac6b7b1bf6c5 223 template<class T>
jocis 1:ac6b7b1bf6c5 224 void attachDirectionChange(T* item, void (T::*method)(int)) {
jocis 1:ac6b7b1bf6c5 225 fPointerDirectionChange.attach( item, method);
jocis 1:ac6b7b1bf6c5 226 }
jocis 1:ac6b7b1bf6c5 227
jocis 1:ac6b7b1bf6c5 228 /** Sets the factor for the getter-functions to convert in another unit (1.0=Hz, 1/(4*CPR)=rps, 1/(60*4*CPR)=rpm, 360/(4*CPR)=°/s, ...)
jocis 1:ac6b7b1bf6c5 229 *
jocis 1:ac6b7b1bf6c5 230 * @param fSpeedFactor - factor to scale from Hz (edges per second = 4 * CPS) to user units
jocis 1:ac6b7b1bf6c5 231 */
jocis 1:ac6b7b1bf6c5 232 void setSpeedFactor(float fSpeedFactor) {
jocis 1:ac6b7b1bf6c5 233 _fSpeedFactor = fSpeedFactor;
jocis 1:ac6b7b1bf6c5 234 }
jocis 1:ac6b7b1bf6c5 235
jocis 1:ac6b7b1bf6c5 236 /** Gets the actual speed as float value. The value is scales by the facor set by setPositionFactor()
jocis 1:ac6b7b1bf6c5 237 *
jocis 1:ac6b7b1bf6c5 238 * @return Actual encoder speed as float
jocis 1:ac6b7b1bf6c5 239 */
jocis 1:ac6b7b1bf6c5 240 float getSpeed();
jocis 1:ac6b7b1bf6c5 241
jocis 1:ac6b7b1bf6c5 242 /** Sets the factor for the getter-functions to convert in another unit (e.g. CPR (cycles per rotation) * 4.0 to get 1.0 for a full rotation)
jocis 1:ac6b7b1bf6c5 243 *
jocis 1:ac6b7b1bf6c5 244 * @param fPositionFactor Factor to scale from counts to user unit
jocis 1:ac6b7b1bf6c5 245 */
jocis 1:ac6b7b1bf6c5 246 void setPositionFactor ( float fPositionFactor ) {
jocis 1:ac6b7b1bf6c5 247 _fPositionFactor = fPositionFactor;
jocis 1:ac6b7b1bf6c5 248 }
jocis 1:ac6b7b1bf6c5 249
jocis 1:ac6b7b1bf6c5 250 /** Gets the actual counter value as float value. The value is scales by the facor set by setSpeedFactor()
jocis 1:ac6b7b1bf6c5 251 *
jocis 1:ac6b7b1bf6c5 252 * @return Actual encoder position as float
jocis 1:ac6b7b1bf6c5 253 */
jocis 1:ac6b7b1bf6c5 254 float getPosition () {
jocis 1:ac6b7b1bf6c5 255 return (float)_counter * _fPositionFactor;
jocis 1:ac6b7b1bf6c5 256 }
jocis 1:ac6b7b1bf6c5 257
jocis 1:ac6b7b1bf6c5 258
jocis 0:46b8d5680f66 259 protected:
jocis 0:46b8d5680f66 260 InterruptIn _pinA, _pinB;
jocis 0:46b8d5680f66 261 DigitalIn _pinI;
jocis 1:ac6b7b1bf6c5 262 FPointer_vi fPointerCounterChange;
jocis 1:ac6b7b1bf6c5 263 FPointer_vi fPointerDirectionChange;
jocis 1:ac6b7b1bf6c5 264
jocis 0:46b8d5680f66 265 int _counter;
jocis 1:ac6b7b1bf6c5 266 short _state;
jocis 1:ac6b7b1bf6c5 267 short _eMode;
jocis 0:46b8d5680f66 268 bool _bZeroOnIndex;
jocis 1:ac6b7b1bf6c5 269
jocis 1:ac6b7b1bf6c5 270 Timer _SpeedTimer;
jocis 1:ac6b7b1bf6c5 271
jocis 1:ac6b7b1bf6c5 272 unsigned int _nSpeedLastTimer;
jocis 1:ac6b7b1bf6c5 273 unsigned int _nSpeedTimeoutMax;
jocis 1:ac6b7b1bf6c5 274 unsigned int _nSpeedTimeoutCount;
jocis 1:ac6b7b1bf6c5 275 int _nSpeedAvrTimeSum;
jocis 1:ac6b7b1bf6c5 276 int _nSpeedAvrTimeCount;
jocis 1:ac6b7b1bf6c5 277 float _fLastSpeed;
jocis 0:46b8d5680f66 278
jocis 1:ac6b7b1bf6c5 279 void ProcessISR ( void );
jocis 1:ac6b7b1bf6c5 280 void callback_timeout();
jocis 1:ac6b7b1bf6c5 281
jocis 1:ac6b7b1bf6c5 282 float _fPositionFactor;
jocis 1:ac6b7b1bf6c5 283 float _fSpeedFactor;
jocis 1:ac6b7b1bf6c5 284
jocis 1:ac6b7b1bf6c5 285 private:
jocis 1:ac6b7b1bf6c5 286 static short _modeLUT[32];
jocis 0:46b8d5680f66 287 };