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
Revision 1:ac6b7b1bf6c5, committed 2014-09-30
- Comitter:
- jocis
- Date:
- Tue Sep 30 12:34:07 2014 +0000
- Parent:
- 0:46b8d5680f66
- Child:
- 2:c0b87b11b9cd
- Commit message:
- Added speed measurement; improved state machine and irq handling
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/FPointer_vi.h Tue Sep 30 12:34:07 2014 +0000
@@ -0,0 +1,143 @@
+/*
+ Copyright (c) 2011 Andy Kirkham
+ modified by Jochen Krapf
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to deal
+ in the Software without restriction, including without limitation the rights
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ THE SOFTWARE.
+*/
+
+#ifndef AJK_FPOINTER_H
+#define AJK_FPOINTER_H
+
+namespace AjK {
+
+class FPointerDummy;
+
+/** FPointer - Adds callbacks that take and return a 32bit uint32_t data type.
+ *
+ * The Mbed library supplies a callback using the FunctionPointer object as
+ * defined in FunctionPointer.h However, this callback system does not allow
+ * the caller to pass a value to the callback. Likewise, the callback itself
+ * cannot return a value.
+ *
+ * FPointer operates in the same way but allows the callback function to be
+ * passed one arg, a uint32_t value. Additionally, the callback can return
+ * a single uint32_t value. The reason for using uint32_t is that the Mbed
+ * and the microcontroller (LPC1768) have a natural data size of 32bits and
+ * this means we can use the uint32_t as a pointer. See example1.h for more
+ * information. This example passes an "int" by passing a pointer to that
+ * int as a 32bit value. Using this technique you can pass any value you like.
+ * All you have to do is pass a pointer to your value cast to (uint32_t). Your
+ * callback can the deference it to get the original value.
+ *
+ * example2.h shows how to do the same thing but demostrates how to specify
+ * the callback into a class object/method.
+ *
+ * Finally, example3.h shows how to pass multiple values. In this example we
+ * define a data structure and in the callback we pass a pointer to that
+ * data structure thus allowing the callback to again get the values.
+ *
+ * Note, when passing pointers to variables to the callback, if the callback
+ * function/method changes that variable's value then it will also change the
+ * value the caller sees. If C pointers are new to you, you are strongly
+ * advised to read up on the subject. It's pointers that often get beginners
+ * into trouble when mis-used.
+ *
+ * @see example1.h
+ * @see example2.h
+ * @see example3.h
+ * @see http://mbed.org/handbook/C-Data-Types
+ * @see http://mbed.org/projects/libraries/svn/mbed/trunk/FunctionPointer.h
+ */
+class FPointer_vi {
+
+protected:
+
+ //! C callback function pointer.
+ void (*c_callback)(int);
+
+ //! C++ callback object/method pointer (the object part).
+ FPointerDummy *obj_callback;
+
+ //! C++ callback object/method pointer (the method part).
+ void (FPointerDummy::*method_callback)(int);
+
+public:
+
+ /** Constructor
+ */
+ FPointer_vi() {
+ c_callback = NULL;
+ obj_callback = NULL;
+ method_callback = NULL;
+ }
+
+ /** attach - Overloaded attachment function.
+ *
+ * Attach a C type function pointer as the callback.
+ *
+ * Note, the callback function prototype must be:-
+ * @code
+ * void myCallbackFunction(int);
+ * @endcode
+ * @param A C function pointer to call.
+ */
+ void attach(void (*function)(int) = 0) { c_callback = function; }
+
+ /** attach - Overloaded attachment function.
+ *
+ * Attach a C++ type object/method pointer as the callback.
+ *
+ * Note, the callback method prototype must be:-
+ * @code
+ * public:
+ * void myCallbackFunction(int);
+ * @endcode
+ * @param A C++ object pointer.
+ * @param A C++ method within the object to call.
+ */
+ template<class T>
+ void attach(T* item, void (T::*method)(int)) {
+ obj_callback = (FPointerDummy *)item;
+ method_callback = (void (FPointerDummy::*)(int))method;
+ }
+
+ /** call - Overloaded callback initiator.
+ *
+ * call the callback function.
+ *
+ * @param int The value to pass to the callback.
+ */
+ void call(int arg) {
+ if (c_callback != NULL) {
+ (*c_callback)(arg);
+ }
+ else {
+ if (obj_callback != NULL && method_callback != NULL) {
+ (obj_callback->*method_callback)(arg);
+ }
+ }
+ }
+
+};
+
+}; // namespace AjK ends
+
+using namespace AjK;
+
+#endif
--- 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
}
+//////////////////////////////////////////////////////////////////////////////////
+
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
--- a/QEIx4.h Tue Sep 02 18:23:36 2014 +0000
+++ b/QEIx4.h Tue Sep 30 12:34:07 2014 +0000
@@ -1,9 +1,10 @@
-/*
+/*
* @author Jochen Krapf
+* parts by Andy Kirkham
*
* @section LICENSE
*
-* Copyright (c) 2012 Jochen Krapf, MIT License
+* Copyright (c) 2014 Jochen Krapf, MIT License
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
* and associated documentation files (the "Software"), to deal in the Software without restriction,
@@ -21,7 +22,7 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* @section DESCRIPTION
-* mbed QEI (quadrature encoder interface) library, for decoding AB signals from a rotary encoder
+* mbed QEI (quadrature encoder interface) library, for decoding AB signals from a rotary encoder
*
* Use cases:
* - Rotary encoder in closed loop motor regulation
@@ -30,13 +31,21 @@
*/
#include "mbed.h"
-
+#include "FPointer_vi.h"
-/** Quadrature encoder interface. A class to decode pulses on a AB rotary encoder. It uses all 4 edges on AB signals to increase resolution 4 times.
+/** === Quadrature Encoder Interface. ===
+*
+* 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)
+*
+* 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.
*
-* In opposite to commen QEI implementation this is resistent to jitter on AB signals and motor vibration. When using interrupts only the needed edge and pin is activated to prevent jamming CPU time with interrups.
+* It can also be used in polling mode i.g. in idle routines if interrupts are not desired.
+*
+* The internal state machine is based on a look up table (LUT) to minimize interrupt retention time and get all necessary flags at once.
*
-* It can also be used in polling mode i.g. in idle routines.
+* 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.
*
* Example:
*
@@ -50,43 +59,51 @@
// ports for nxp LPC 1768
QEIx4 qei1(p30, p29, p28); // QEI with index signal for zeroing
QEIx4 qei2(p27, p26, NC); // QEI only with AB signals
-QEIx4 qei3(p25, p24, NC, false); // QEI without interrups for polling mode
+QEIx4 qei3(p25, p24, NC, QEIx4::POLLING); // QEI without interrups for polling mode
int main() {
t.start();
qei1.SetZeroOnIndex(true); // Set the flag to zero counter on next index signal rises
- while(1)
+ while(1)
{
qei3.poll(); // poll manually without interrupt - sampling in this loop with about 2kHz
-
+
if ( t.read_ms() > 500 ) // every half second
{
t.reset();
t.start();
myled = !myled;
-
+
printf ( "\r\n%6d %6d %6d", (int)qei1, (int)qei2, (int)qei3 ); // print counter values
}
- wait_us(500); // for about 2kHz
+ wait_us(20); // for about 50kHz
}
}
* @endcode
*/
-class QEIx4 {
+class QEIx4
+{
public:
+ typedef enum EMODE {
+ POLLING = 0,
+ IRQ = 1,
+ IRQ_NO_JAMMING = 2,
+ SPEED = 4,
+ } EMODE;
+
/** constructor of QEIx4 object
*
* @param pinA Pin number of input/interrupt pin for encoder line A. All port pins are possible except p19 and p20
* @param pinB Pin number of input/interrupt pin for encoder line B. All port pins are possible except p19 and p20
* @param pinI Pin number of input pin for optional encoder index or reference switch.
- * @param bUseIRQ Flag to use interrups to detect changes on line A and B. If FALSE the function poll() has to be called frequently
+ * @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
*/
- QEIx4 ( PinName pinA, PinName pinB, PinName pinI=NC, bool bUseIRQ=true );
-
+ QEIx4 ( PinName pinA, PinName pinB, PinName pinI=NC, EMODE eMode=IRQ );
+
/** destructor of QEIx4 object
*/
~QEIx4();
@@ -95,57 +112,49 @@
*
* @return Actual counter value
*/
- int read()
- { return _counter; }
+ int read() {
+ return _counter;
+ }
/** Gets the actual counter value as int operator.
*
* @return Actual counter value as int operator
*/
- operator int () // int-Operator
- { return _counter; }
-
+ operator int () { // int-Operator
+ return _counter;
+ }
+
/** Sets the counter value at actual encoder position to given value.
*
* @param Counter value
*/
- void write ( int counter )
- { _counter = counter; }
-
+ void write ( int counter ) {
+ _counter = counter;
+ }
+
/** Sets the counter value at actual encoder position to given value as assign operator.
*
* @param Counter value
*/
- int operator= ( int counter ) // Assign-Operator
- { write(counter); return counter; }
-
+ int operator= ( int counter ) { // Assign-Operator
+ write(counter);
+ return counter;
+ }
+
/** Polls the state machine manually and updates the counter value.
*/
- void poll ()
- { ProcessISR(); }
-
- /** Gets the actual counter value as float value. A value of 1.0 is one rotation
- *
- * @return Actual encoder position as float
- */
- float GetPosition ()
- { return (float)_counter / (_cpr*4.0); }
-
- /** Sets the number of cycles per rotation (CPR) for function GetPosition().
- *
- * @param Cycles per rotation for a full rotation
- */
- void SetCPR ( int cpr )
- { _cpr = cpr; }
-
+ void poll () {
+ ProcessISR();
+ }
+
/** 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
*
* @param Flag for triggering. Is reset on next zeroing
*/
- void SetZeroOnIndex ( bool bZeroOnIndex )
- { _bZeroOnIndex = bZeroOnIndex; }
+ void setZeroOnIndex ( bool bZeroOnIndex ) {
+ _bZeroOnIndex = bZeroOnIndex;
+ }
-
/** Callback in derived classes to act on counter change
*/
virtual void CounterChange ( void ) {};
@@ -153,18 +162,126 @@
/** Callback in derived classes to act on zeroing trigger
*/
virtual void CounterZero ( void ) {};
-
+
+ /** attach - Overloaded attachment function.
+ *
+ * Attach a C type function pointer as the callback.
+ *
+ * Note, the callback function prototype must be:-
+ * @code
+ * void myCallbackFunction(int);
+ * @endcode
+ * @param A C function pointer to call.
+ */
+ void attachCounterChange(void (*function)(int) = 0) {
+ fPointerCounterChange.attach (function);
+ }
+
+ /** attachCounterChange - Overloaded attachment function.
+ *
+ * Attach a C++ type object/method pointer as the callback.
+ *
+ * Note, the callback method prototype must be:-
+ * @code
+ * public:
+ * static void myCallbackFunction(int);
+ * @endcode
+ * @param A C++ object pointer.
+ * @param A C++ method within the object to call.
+ */
+ template<class T>
+ void attachCounterChange(T* item, void (T::*method)(int)) {
+ fPointerCounterChange.attach( item, method);
+ }
+
+ /** attachDirectionChange - Overloaded attachment function.
+ *
+ * Attach a C type function pointer as the callback.
+ *
+ * Note, the callback function prototype must be:-
+ * @code
+ * void myCallbackFunction(int);
+ * @endcode
+ * @param A C function pointer to call.
+ */
+ void attachDirectionChange(void (*function)(int) = 0) {
+ fPointerDirectionChange.attach (function);
+ }
+
+ /** attachDirectionChange - Overloaded attachment function.
+ *
+ * Attach a C++ type object/method pointer as the callback.
+ *
+ * Note, the callback method prototype must be:-
+ * @code
+ * public:
+ * static void myCallbackFunction(int);
+ * @endcode
+ * @param A C++ object pointer.
+ * @param A C++ method within the object to call.
+ */
+ template<class T>
+ void attachDirectionChange(T* item, void (T::*method)(int)) {
+ fPointerDirectionChange.attach( item, method);
+ }
+
+ /** 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, ...)
+ *
+ * @param fSpeedFactor - factor to scale from Hz (edges per second = 4 * CPS) to user units
+ */
+ void setSpeedFactor(float fSpeedFactor) {
+ _fSpeedFactor = fSpeedFactor;
+ }
+
+ /** Gets the actual speed as float value. The value is scales by the facor set by setPositionFactor()
+ *
+ * @return Actual encoder speed as float
+ */
+ float getSpeed();
+
+ /** 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)
+ *
+ * @param fPositionFactor Factor to scale from counts to user unit
+ */
+ void setPositionFactor ( float fPositionFactor ) {
+ _fPositionFactor = fPositionFactor;
+ }
+
+ /** Gets the actual counter value as float value. The value is scales by the facor set by setSpeedFactor()
+ *
+ * @return Actual encoder position as float
+ */
+ float getPosition () {
+ return (float)_counter * _fPositionFactor;
+ }
+
+
protected:
InterruptIn _pinA, _pinB;
DigitalIn _pinI;
+ FPointer_vi fPointerCounterChange;
+ FPointer_vi fPointerDirectionChange;
+
int _counter;
- short _mode;
- int _cpr;
- bool _bUseIRQ;
+ short _state;
+ short _eMode;
bool _bZeroOnIndex;
-
- void ProcessISR ( void );
+
+ Timer _SpeedTimer;
+
+ unsigned int _nSpeedLastTimer;
+ unsigned int _nSpeedTimeoutMax;
+ unsigned int _nSpeedTimeoutCount;
+ int _nSpeedAvrTimeSum;
+ int _nSpeedAvrTimeCount;
+ float _fLastSpeed;
-private:
- static short _modeLUT[16];
+ void ProcessISR ( void );
+ void callback_timeout();
+
+ float _fPositionFactor;
+ float _fSpeedFactor;
+
+private:
+ static short _modeLUT[32];
};
\ No newline at end of file