OpenMoCo
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:
(+) Count UP; (-) Count DOWN
Diff: QEIx4.cpp
- Revision:
- 1:ac6b7b1bf6c5
- Parent:
- 0:46b8d5680f66
- Child:
- 2:c0b87b11b9cd
--- a/QEIx4.cpp Tue Sep 02 18:23:36 2014 +0000
+++ b/QEIx4.cpp Tue Sep 30 12:34:07 2014 +0000
@@ -1,38 +1,115 @@
#include "QEIx4.h"
-// state machine for decoting - don't change!!!
-short QEIx4::_modeLUT[16] = { 0, 21, 2, 47, 4, 5, 34, 27, 36, 9, 30, 11, 16, 41, 14, 15 };
-
// bit masks for state machine - don't change!!!
-#define QEIx4_MASK 0xC
-#define QEIx4_INC 0x10
-#define QEIx4_DEC 0x20
-#define QEIx4_CHG 0x30
+#define QEIx4_STATE 0xC
+#define QEIx4_MASK 0x1C
+#define QEIx4_INC 0x40
+#define QEIx4_DEC 0x80
+#define QEIx4_CHG 0xC0
+#define QEIx4_DIR 0x20
#define QEIx4_A 1
#define QEIx4_B 2
+#define QEIx4_AB 3
#define QEIx4_S0 0x0
#define QEIx4_S1 0x4
#define QEIx4_S2 0x8
#define QEIx4_S3 0xC
+#define QEIx4_CCW 0
+#define QEIx4_CW 0x10
+
+// state machine for decoting - don't change!!!
+short QEIx4::_modeLUT[32] = {
+ // act state S0 in CCW direction
+ QEIx4_CCW | QEIx4_S0,
+ QEIx4_CW | QEIx4_S1 | QEIx4_A | QEIx4_INC | QEIx4_DIR,
+ QEIx4_CCW | QEIx4_S0 | QEIx4_B,
+ QEIx4_CCW | QEIx4_S3 | QEIx4_AB | QEIx4_DEC,
+ // act state S1 in CCW direction
+ QEIx4_CCW | QEIx4_S1,
+ QEIx4_CCW | QEIx4_S1 | QEIx4_A,
+ QEIx4_CCW | QEIx4_S0 | QEIx4_B | QEIx4_DEC,
+ QEIx4_CW | QEIx4_S2 | QEIx4_AB | QEIx4_INC | QEIx4_DIR,
+ // act state S2 in CCW direction
+ QEIx4_CCW | QEIx4_S1 | QEIx4_DEC,
+ QEIx4_CCW | QEIx4_S2 | QEIx4_A,
+ QEIx4_CW | QEIx4_S3 | QEIx4_B | QEIx4_INC | QEIx4_DIR,
+ QEIx4_CCW | QEIx4_S2 | QEIx4_AB,
+ // act state S3 in CCW direction
+ QEIx4_CW | QEIx4_S0 | QEIx4_INC | QEIx4_DIR,
+ QEIx4_CCW | QEIx4_S2 | QEIx4_A | QEIx4_DEC,
+ QEIx4_CCW | QEIx4_S3 | QEIx4_B,
+ QEIx4_CCW | QEIx4_S3 | QEIx4_AB,
+
+ // act state S0 in CW direction
+ QEIx4_CW | QEIx4_S0,
+ QEIx4_CW | QEIx4_S1 | QEIx4_A | QEIx4_INC,
+ QEIx4_CW | QEIx4_S0 | QEIx4_B,
+ QEIx4_CCW | QEIx4_S3 | QEIx4_AB | QEIx4_DEC | QEIx4_DIR,
+ // act state S1 in CW direction
+ QEIx4_CW | QEIx4_S1,
+ QEIx4_CW | QEIx4_S1 | QEIx4_A,
+ QEIx4_CCW | QEIx4_S0 | QEIx4_B | QEIx4_DEC | QEIx4_DIR,
+ QEIx4_CW | QEIx4_S2 | QEIx4_AB | QEIx4_INC,
+ // act state S2 in CW direction
+ QEIx4_CCW | QEIx4_S1 | QEIx4_DEC | QEIx4_DIR,
+ QEIx4_CW | QEIx4_S2 | QEIx4_A,
+ QEIx4_CW | QEIx4_S3 | QEIx4_B | QEIx4_INC,
+ QEIx4_CW | QEIx4_S2 | QEIx4_AB,
+ // act state S3 in CW direction
+ QEIx4_CW | QEIx4_S0 | QEIx4_INC,
+ QEIx4_CCW | QEIx4_S2 | QEIx4_A | QEIx4_DEC | QEIx4_DIR,
+ QEIx4_CW | QEIx4_S3 | QEIx4_B,
+ QEIx4_CW | QEIx4_S3 | QEIx4_AB
+};
+
//#define DEB(x) printf (x)
#define DEB(x)
///////////////////////////////////////////////////////////////////////////////
-QEIx4::QEIx4 ( PinName pinA, PinName pinB, PinName pinI, bool bUseIRQ ) : _pinA(pinA), _pinB(pinB), _pinI(pinI)
+QEIx4::QEIx4 ( PinName pinA, PinName pinB, PinName pinI, EMODE eMode ) : _pinA(pinA), _pinB(pinB), _pinI(pinI)
{
- _bUseIRQ = bUseIRQ;
+ _eMode = eMode;
_bZeroOnIndex = false;
- _cpr = 500;
-
+ _fPositionFactor = 1.0;
+
// synchronize state machine
_counter = 0;
- ProcessISR ();
- _mode += QEIx4_S2;
+ _state = 0;
+
+ _pinA.mode(PullUp);
+ _pinB.mode(PullUp);
+ if(pinI!=NC)
+ _pinI.mode(PullUp);
+
+ if ( _eMode & IRQ ) {
+ _pinA.fall ( this, &QEIx4::ProcessISR );
+ _pinA.rise ( this, &QEIx4::ProcessISR );
+ _pinB.fall ( this, &QEIx4::ProcessISR );
+ _pinB.rise ( this, &QEIx4::ProcessISR );
+ }
+ if ( _eMode & IRQ_NO_JAMMING ) {
+ ProcessISR ();
+ _state += QEIx4_S2;
+ }
ProcessISR ();
_counter = 0;
-
+
+ if ( _eMode & SPEED ) {
+ _SpeedTimer.reset();
+ _SpeedTimer.start();
+ }
+
+ _nSpeedLastTimer = 0;
+ _nSpeedAvrTimeSum = 0;
+ _nSpeedAvrTimeCount = -1;
+ _fSpeedFactor = 1.0f;
+ _fLastSpeed = 0;
+
+ _nSpeedTimeoutMax = 10;
+ _nSpeedTimeoutCount = 0;
+
}
///////////////////////////////////////////////////////////////////////////////
@@ -47,75 +124,129 @@
///////////////////////////////////////////////////////////////////////////////
-void QEIx4::ProcessISR ( void )
+float QEIx4::getSpeed()
+{
+ float fSpeed;
+ int avrTimeSum = 0;
+ int avrTimeCount = 0;
+
+ __disable_irq(); // Disable Interrupts for atomic copy
+ avrTimeSum = _nSpeedAvrTimeSum;
+ avrTimeCount = _nSpeedAvrTimeCount;
+ _nSpeedAvrTimeSum = 0;
+ _nSpeedAvrTimeCount = 0;
+ __enable_irq(); // Enable Interrupts
+
+ if ( avrTimeCount == 0 ) {
+ if (_nSpeedTimeoutCount++ > _nSpeedTimeoutMax)
+ _fLastSpeed *= 0.5f;
+ fSpeed = _fLastSpeed;
+ } else if ( avrTimeCount < 0 || avrTimeSum == 0 ) {
+ fSpeed = 0;
+ _nSpeedTimeoutCount = 0;
+ } else {
+ fSpeed = 1000000.0f * _fSpeedFactor / ( (float)avrTimeSum / (float)avrTimeCount );
+ _nSpeedTimeoutCount = 0;
+ }
+ _fLastSpeed = fSpeed;
+
+ return fSpeed;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+
+void QEIx4::ProcessISR ( void )
{
- DEB (".");
- _mode &= QEIx4_MASK;
- if ( _pinA ) _mode |= QEIx4_A;
- if ( _pinB ) _mode |= QEIx4_B;
-
- _mode = _modeLUT[_mode];
-
- if ( _mode & QEIx4_CHG )
- {
- bool bCounterChange = false;
-
- if ( _mode & QEIx4_INC )
- {
- _counter++;
- bCounterChange = true;
- }
- if ( _mode & QEIx4_DEC )
- {
- _counter--;
- bCounterChange = true;
- }
- if ( _bZeroOnIndex && _pinI != NC && _pinI == 1 )
- {
- _counter = 0;
- _bZeroOnIndex = false;
- CounterZero ();
- bCounterChange = true;
+ int pinA, pinB;
+
+ do {
+ pinA = _pinA;
+ pinB = _pinB;
+
+ DEB (".");
+ _state &= QEIx4_MASK;
+ if ( pinA ) _state |= QEIx4_A;
+ if ( pinB ) _state |= QEIx4_B;
+
+ _state = _modeLUT[_state]; // magic is done by lookup-table
+
+ if ( _state & QEIx4_CHG ) { // is any change?
+ bool bCounterChange = false;
+
+ if ( _state & QEIx4_INC ) { // is moved foreward?
+ _counter++;
+ bCounterChange = true;
+ }
+ if ( _state & QEIx4_DEC ) { // is moved backward?
+ _counter--;
+ bCounterChange = true;
+ }
+ if ( _bZeroOnIndex && _pinI != NC && _pinI == 1 ) { // is index pin triggered?
+ _counter = 0;
+ _bZeroOnIndex = false;
+ CounterZero ();
+ bCounterChange = true;
+ }
+
+ if ( _eMode & IRQ_NO_JAMMING ) { // need reconfiguration od interrupt edges?
+ switch ( _state & QEIx4_STATE ) {
+ case QEIx4_S0:
+ _pinB.rise ( NULL );
+ _pinB.fall ( NULL );
+ _pinA.rise ( this, &QEIx4::ProcessISR );
+ DEB ("S0");
+ break;
+ case QEIx4_S1:
+ _pinA.rise ( NULL );
+ _pinA.fall ( NULL );
+ _pinB.rise ( this, &QEIx4::ProcessISR );
+ DEB ("S1");
+ break;
+ case QEIx4_S2:
+ _pinB.rise ( NULL );
+ _pinB.fall ( NULL );
+ _pinA.fall ( this, &QEIx4::ProcessISR );
+ DEB ("S2");
+ break;
+ case QEIx4_S3:
+ _pinA.rise ( NULL );
+ _pinA.fall ( NULL );
+ _pinB.fall ( this, &QEIx4::ProcessISR );
+ DEB ("S3");
+ break;
+ }
+ }
+
+ if ( _eMode & SPEED ) {
+ unsigned int act = _SpeedTimer.read_us();
+ unsigned int diff = act - _nSpeedLastTimer; // Note: overflow is handled correctly
+ _nSpeedLastTimer = act;
+
+ if ( _nSpeedAvrTimeCount < 0 ) { // ignore first pulse to synchronize timer (maybe timer overflow)
+ _nSpeedAvrTimeSum = 0;
+ _nSpeedAvrTimeCount = 0;
+ } else {
+ if ( _state & QEIx4_CW ) // is moving foreward?
+ _nSpeedAvrTimeSum += diff;
+ else
+ _nSpeedAvrTimeSum -= diff;
+ _nSpeedAvrTimeCount++;
+ }
+ }
+
+ if ( bCounterChange ) { // has counter changed?
+ CounterChange ();
+ fPointerCounterChange.call(_counter);
+ if ( _state & QEIx4_DIR )
+ fPointerDirectionChange.call(_counter);
+ }
+
}
-
- if ( _bUseIRQ )
- {
- switch ( _mode & QEIx4_MASK )
- {
- case QEIx4_S0:
- _pinB.rise ( NULL );
- _pinB.fall ( NULL );
- _pinA.rise ( this, &QEIx4::ProcessISR );
- DEB ("S0");
- break;
- case QEIx4_S1:
- _pinA.rise ( NULL );
- _pinA.fall ( NULL );
- _pinB.rise ( this, &QEIx4::ProcessISR );
- DEB ("S1");
- break;
- case QEIx4_S2:
- _pinB.rise ( NULL );
- _pinB.fall ( NULL );
- _pinA.fall ( this, &QEIx4::ProcessISR );
- DEB ("S2");
- break;
- case QEIx4_S3:
- _pinA.rise ( NULL );
- _pinA.fall ( NULL );
- _pinB.fall ( this, &QEIx4::ProcessISR );
- DEB ("S3");
- break;
- }
- }
-
- if ( bCounterChange )
- {
- CounterChange ();
- }
- }
+ } while ( pinA != _pinA || pinB != _pinB); // loop till stable input pins
}
+//////////////////////////////////////////////////////////////////////////////////
+
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////