QEI - | Mbed

Users » farbodjam » Code » QEI

QEI.cpp@0:fc0482177a6d, 2011-08-27 (annotated)

Committer:: farbodjam
Date:: Sat Aug 27 12:47:06 2011 +0000
Revision:: 0:fc0482177a6d

Who changed what in which revision?

User	Revision	Line number	New contents of line
farbodjam	0:fc0482177a6d	1	/**
farbodjam	0:fc0482177a6d	2	* @author Aaron Berk
farbodjam	0:fc0482177a6d	3	*
farbodjam	0:fc0482177a6d	4	* @section LICENSE
farbodjam	0:fc0482177a6d	5	*
farbodjam	0:fc0482177a6d	6	* Copyright (c) 2010 ARM Limited
farbodjam	0:fc0482177a6d	7	*
farbodjam	0:fc0482177a6d	8	* Permission is hereby granted, free of charge, to any person obtaining a copy
farbodjam	0:fc0482177a6d	9	* of this software and associated documentation files (the "Software"), to deal
farbodjam	0:fc0482177a6d	10	* in the Software without restriction, including without limitation the rights
farbodjam	0:fc0482177a6d	11	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
farbodjam	0:fc0482177a6d	12	* copies of the Software, and to permit persons to whom the Software is
farbodjam	0:fc0482177a6d	13	* furnished to do so, subject to the following conditions:
farbodjam	0:fc0482177a6d	14	*
farbodjam	0:fc0482177a6d	15	* The above copyright notice and this permission notice shall be included in
farbodjam	0:fc0482177a6d	16	* all copies or substantial portions of the Software.
farbodjam	0:fc0482177a6d	17	*
farbodjam	0:fc0482177a6d	18	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
farbodjam	0:fc0482177a6d	19	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
farbodjam	0:fc0482177a6d	20	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
farbodjam	0:fc0482177a6d	21	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
farbodjam	0:fc0482177a6d	22	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
farbodjam	0:fc0482177a6d	23	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
farbodjam	0:fc0482177a6d	24	* THE SOFTWARE.
farbodjam	0:fc0482177a6d	25	*
farbodjam	0:fc0482177a6d	26	* @section DESCRIPTION
farbodjam	0:fc0482177a6d	27	*
farbodjam	0:fc0482177a6d	28	* Quadrature Encoder Interface.
farbodjam	0:fc0482177a6d	29	*
farbodjam	0:fc0482177a6d	30	* A quadrature encoder consists of two code tracks on a disc which are 90
farbodjam	0:fc0482177a6d	31	* degrees out of phase. It can be used to determine how far a wheel has
farbodjam	0:fc0482177a6d	32	* rotated, relative to a known starting position.
farbodjam	0:fc0482177a6d	33	*
farbodjam	0:fc0482177a6d	34	* Only one code track changes at a time leading to a more robust system than
farbodjam	0:fc0482177a6d	35	* a single track, because any jitter around any edge won't cause a state
farbodjam	0:fc0482177a6d	36	* change as the other track will remain constant.
farbodjam	0:fc0482177a6d	37	*
farbodjam	0:fc0482177a6d	38	* Encoders can be a homebrew affair, consisting of infrared emitters/receivers
farbodjam	0:fc0482177a6d	39	* and paper code tracks consisting of alternating black and white sections;
farbodjam	0:fc0482177a6d	40	* alternatively, complete disk and PCB emitter/receiver encoder systems can
farbodjam	0:fc0482177a6d	41	* be bought, but the interface, regardless of implementation is the same.
farbodjam	0:fc0482177a6d	42	*
farbodjam	0:fc0482177a6d	43	* +-----+ +-----+ +-----+
farbodjam	0:fc0482177a6d	44	* Channel A \| ^ \| \| \| \| \|
farbodjam	0:fc0482177a6d	45	* ---+ ^ +-----+ +-----+ +-----
farbodjam	0:fc0482177a6d	46	* ^ ^
farbodjam	0:fc0482177a6d	47	* ^ +-----+ +-----+ +-----+
farbodjam	0:fc0482177a6d	48	* Channel B ^ \| \| \| \| \| \|
farbodjam	0:fc0482177a6d	49	* ------+ +-----+ +-----+ +-----
farbodjam	0:fc0482177a6d	50	* ^ ^
farbodjam	0:fc0482177a6d	51	* ^ ^
farbodjam	0:fc0482177a6d	52	* 90deg
farbodjam	0:fc0482177a6d	53	*
farbodjam	0:fc0482177a6d	54	* The interface uses X2 encoding by default which calculates the pulse count
farbodjam	0:fc0482177a6d	55	* based on reading the current state after each rising and falling edge of
farbodjam	0:fc0482177a6d	56	* channel A.
farbodjam	0:fc0482177a6d	57	*
farbodjam	0:fc0482177a6d	58	* +-----+ +-----+ +-----+
farbodjam	0:fc0482177a6d	59	* Channel A \| \| \| \| \| \|
farbodjam	0:fc0482177a6d	60	* ---+ +-----+ +-----+ +-----
farbodjam	0:fc0482177a6d	61	* ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	62	* ^ +-----+ ^ +-----+ ^ +-----+
farbodjam	0:fc0482177a6d	63	* Channel B ^ \| ^ \| ^ \| ^ \| ^ \| \|
farbodjam	0:fc0482177a6d	64	* ------+ ^ +-----+ ^ +-----+ +--
farbodjam	0:fc0482177a6d	65	* ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	66	* ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	67	* Pulse count 0 1 2 3 4 5 ...
farbodjam	0:fc0482177a6d	68	*
farbodjam	0:fc0482177a6d	69	* This interface can also use X4 encoding which calculates the pulse count
farbodjam	0:fc0482177a6d	70	* based on reading the current state after each rising and falling edge of
farbodjam	0:fc0482177a6d	71	* either channel.
farbodjam	0:fc0482177a6d	72	*
farbodjam	0:fc0482177a6d	73	* +-----+ +-----+ +-----+
farbodjam	0:fc0482177a6d	74	* Channel A \| \| \| \| \| \|
farbodjam	0:fc0482177a6d	75	* ---+ +-----+ +-----+ +-----
farbodjam	0:fc0482177a6d	76	* ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	77	* ^ +-----+ ^ +-----+ ^ +-----+
farbodjam	0:fc0482177a6d	78	* Channel B ^ \| ^ \| ^ \| ^ \| ^ \| \|
farbodjam	0:fc0482177a6d	79	* ------+ ^ +-----+ ^ +-----+ +--
farbodjam	0:fc0482177a6d	80	* ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	81	* ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	82	* Pulse count 0 1 2 3 4 5 6 7 8 9 ...
farbodjam	0:fc0482177a6d	83	*
farbodjam	0:fc0482177a6d	84	* It defaults
farbodjam	0:fc0482177a6d	85	*
farbodjam	0:fc0482177a6d	86	* An optional index channel can be used which determines when a full
farbodjam	0:fc0482177a6d	87	* revolution has occured.
farbodjam	0:fc0482177a6d	88	*
farbodjam	0:fc0482177a6d	89	* If a 4 pules per revolution encoder was used, with X4 encoding,
farbodjam	0:fc0482177a6d	90	* the following would be observed.
farbodjam	0:fc0482177a6d	91	*
farbodjam	0:fc0482177a6d	92	* +-----+ +-----+ +-----+
farbodjam	0:fc0482177a6d	93	* Channel A \| \| \| \| \| \|
farbodjam	0:fc0482177a6d	94	* ---+ +-----+ +-----+ +-----
farbodjam	0:fc0482177a6d	95	* ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	96	* ^ +-----+ ^ +-----+ ^ +-----+
farbodjam	0:fc0482177a6d	97	* Channel B ^ \| ^ \| ^ \| ^ \| ^ \| \|
farbodjam	0:fc0482177a6d	98	* ------+ ^ +-----+ ^ +-----+ +--
farbodjam	0:fc0482177a6d	99	* ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	100	* ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	101	* ^ ^ ^ +--+ ^ ^ +--+ ^
farbodjam	0:fc0482177a6d	102	* ^ ^ ^ \| \| ^ ^ \| \| ^
farbodjam	0:fc0482177a6d	103	* Index ------------+ +--------+ +-----------
farbodjam	0:fc0482177a6d	104	* ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
farbodjam	0:fc0482177a6d	105	* Pulse count 0 1 2 3 4 5 6 7 8 9 ...
farbodjam	0:fc0482177a6d	106	* Rev. count 0 1 2
farbodjam	0:fc0482177a6d	107	*
farbodjam	0:fc0482177a6d	108	* Rotational position in degrees can be calculated by:
farbodjam	0:fc0482177a6d	109	*
farbodjam	0:fc0482177a6d	110	* (pulse count / X * N) * 360
farbodjam	0:fc0482177a6d	111	*
farbodjam	0:fc0482177a6d	112	* Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
farbodjam	0:fc0482177a6d	113	* of pulses per revolution.
farbodjam	0:fc0482177a6d	114	*
farbodjam	0:fc0482177a6d	115	* Linear position can be calculated by:
farbodjam	0:fc0482177a6d	116	*
farbodjam	0:fc0482177a6d	117	* (pulse count / X * N) * (1 / PPI)
farbodjam	0:fc0482177a6d	118	*
farbodjam	0:fc0482177a6d	119	* Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
farbodjam	0:fc0482177a6d	120	* pulses per revolution, and PPI is pulses per inch, or the equivalent for
farbodjam	0:fc0482177a6d	121	* any other unit of displacement. PPI can be calculated by taking the
farbodjam	0:fc0482177a6d	122	* circumference of the wheel or encoder disk and dividing it by the number
farbodjam	0:fc0482177a6d	123	* of pulses per revolution.
farbodjam	0:fc0482177a6d	124	*/
farbodjam	0:fc0482177a6d	125
farbodjam	0:fc0482177a6d	126	/**
farbodjam	0:fc0482177a6d	127	* Includes
farbodjam	0:fc0482177a6d	128	*/
farbodjam	0:fc0482177a6d	129	#include "QEI.h"
farbodjam	0:fc0482177a6d	130
farbodjam	0:fc0482177a6d	131	QEI::QEI(PinName channelA,
farbodjam	0:fc0482177a6d	132	PinName channelB,
farbodjam	0:fc0482177a6d	133	PinName index,
farbodjam	0:fc0482177a6d	134	int pulsesPerRev,
farbodjam	0:fc0482177a6d	135	Encoding encoding) : channelA_(channelA), channelB_(channelB),
farbodjam	0:fc0482177a6d	136	index_(index) {
farbodjam	0:fc0482177a6d	137
farbodjam	0:fc0482177a6d	138	pulses_ = 0;
farbodjam	0:fc0482177a6d	139	revolutions_ = 0;
farbodjam	0:fc0482177a6d	140	pulsesPerRev_ = pulsesPerRev;
farbodjam	0:fc0482177a6d	141	encoding_ = encoding;
farbodjam	0:fc0482177a6d	142
farbodjam	0:fc0482177a6d	143	//Workout what the current state is.
farbodjam	0:fc0482177a6d	144	int chanA = channelA_.read();
farbodjam	0:fc0482177a6d	145	int chanB = channelB_.read();
farbodjam	0:fc0482177a6d	146
farbodjam	0:fc0482177a6d	147	//2-bit state.
farbodjam	0:fc0482177a6d	148	currState_ = (chanA << 1) \| (chanB);
farbodjam	0:fc0482177a6d	149	prevState_ = currState_;
farbodjam	0:fc0482177a6d	150
farbodjam	0:fc0482177a6d	151	//X2 encoding uses interrupts on only channel A.
farbodjam	0:fc0482177a6d	152	//X4 encoding uses interrupts on channel A,
farbodjam	0:fc0482177a6d	153	//and on channel B.
farbodjam	0:fc0482177a6d	154	channelA_.rise(this, &QEI::encode);
farbodjam	0:fc0482177a6d	155	channelA_.fall(this, &QEI::encode);
farbodjam	0:fc0482177a6d	156
farbodjam	0:fc0482177a6d	157	//If we're using X4 encoding, then attach interrupts to channel B too.
farbodjam	0:fc0482177a6d	158	if (encoding == X4_ENCODING) {
farbodjam	0:fc0482177a6d	159	channelB_.rise(this, &QEI::encode);
farbodjam	0:fc0482177a6d	160	channelB_.fall(this, &QEI::encode);
farbodjam	0:fc0482177a6d	161	}
farbodjam	0:fc0482177a6d	162	//Index is optional.
farbodjam	0:fc0482177a6d	163	if (index != NC) {
farbodjam	0:fc0482177a6d	164	index_.rise(this, &QEI::index);
farbodjam	0:fc0482177a6d	165	}
farbodjam	0:fc0482177a6d	166
farbodjam	0:fc0482177a6d	167	}
farbodjam	0:fc0482177a6d	168
farbodjam	0:fc0482177a6d	169	void QEI::reset(void) {
farbodjam	0:fc0482177a6d	170
farbodjam	0:fc0482177a6d	171	pulses_ = 0;
farbodjam	0:fc0482177a6d	172	revolutions_ = 0;
farbodjam	0:fc0482177a6d	173
farbodjam	0:fc0482177a6d	174	}
farbodjam	0:fc0482177a6d	175
farbodjam	0:fc0482177a6d	176	int QEI::getCurrentState(void) {
farbodjam	0:fc0482177a6d	177
farbodjam	0:fc0482177a6d	178	return currState_;
farbodjam	0:fc0482177a6d	179
farbodjam	0:fc0482177a6d	180	}
farbodjam	0:fc0482177a6d	181
farbodjam	0:fc0482177a6d	182	int QEI::getPulses(void) {
farbodjam	0:fc0482177a6d	183
farbodjam	0:fc0482177a6d	184	return pulses_;
farbodjam	0:fc0482177a6d	185
farbodjam	0:fc0482177a6d	186	}
farbodjam	0:fc0482177a6d	187
farbodjam	0:fc0482177a6d	188	int QEI::getRevolutions(void) {
farbodjam	0:fc0482177a6d	189
farbodjam	0:fc0482177a6d	190	return revolutions_;
farbodjam	0:fc0482177a6d	191
farbodjam	0:fc0482177a6d	192	}
farbodjam	0:fc0482177a6d	193
farbodjam	0:fc0482177a6d	194	// +-------------+
farbodjam	0:fc0482177a6d	195	// \| X2 Encoding \|
farbodjam	0:fc0482177a6d	196	// +-------------+
farbodjam	0:fc0482177a6d	197	//
farbodjam	0:fc0482177a6d	198	// When observing states two patterns will appear:
farbodjam	0:fc0482177a6d	199	//
farbodjam	0:fc0482177a6d	200	// Counter clockwise rotation:
farbodjam	0:fc0482177a6d	201	//
farbodjam	0:fc0482177a6d	202	// 10 -> 01 -> 10 -> 01 -> ...
farbodjam	0:fc0482177a6d	203	//
farbodjam	0:fc0482177a6d	204	// Clockwise rotation:
farbodjam	0:fc0482177a6d	205	//
farbodjam	0:fc0482177a6d	206	// 11 -> 00 -> 11 -> 00 -> ...
farbodjam	0:fc0482177a6d	207	//
farbodjam	0:fc0482177a6d	208	// We consider counter clockwise rotation to be "forward" and
farbodjam	0:fc0482177a6d	209	// counter clockwise to be "backward". Therefore pulse count will increase
farbodjam	0:fc0482177a6d	210	// during counter clockwise rotation and decrease during clockwise rotation.
farbodjam	0:fc0482177a6d	211	//
farbodjam	0:fc0482177a6d	212	// +-------------+
farbodjam	0:fc0482177a6d	213	// \| X4 Encoding \|
farbodjam	0:fc0482177a6d	214	// +-------------+
farbodjam	0:fc0482177a6d	215	//
farbodjam	0:fc0482177a6d	216	// There are four possible states for a quadrature encoder which correspond to
farbodjam	0:fc0482177a6d	217	// 2-bit gray code.
farbodjam	0:fc0482177a6d	218	//
farbodjam	0:fc0482177a6d	219	// A state change is only valid if of only one bit has changed.
farbodjam	0:fc0482177a6d	220	// A state change is invalid if both bits have changed.
farbodjam	0:fc0482177a6d	221	//
farbodjam	0:fc0482177a6d	222	// Clockwise Rotation ->
farbodjam	0:fc0482177a6d	223	//
farbodjam	0:fc0482177a6d	224	// 00 01 11 10 00
farbodjam	0:fc0482177a6d	225	//
farbodjam	0:fc0482177a6d	226	// <- Counter Clockwise Rotation
farbodjam	0:fc0482177a6d	227	//
farbodjam	0:fc0482177a6d	228	// If we observe any valid state changes going from left to right, we have
farbodjam	0:fc0482177a6d	229	// moved one pulse clockwise [we will consider this "backward" or "negative"].
farbodjam	0:fc0482177a6d	230	//
farbodjam	0:fc0482177a6d	231	// If we observe any valid state changes going from right to left we have
farbodjam	0:fc0482177a6d	232	// moved one pulse counter clockwise [we will consider this "forward" or
farbodjam	0:fc0482177a6d	233	// "positive"].
farbodjam	0:fc0482177a6d	234	//
farbodjam	0:fc0482177a6d	235	// We might enter an invalid state for a number of reasons which are hard to
farbodjam	0:fc0482177a6d	236	// predict - if this is the case, it is generally safe to ignore it, update
farbodjam	0:fc0482177a6d	237	// the state and carry on, with the error correcting itself shortly after.
farbodjam	0:fc0482177a6d	238	void QEI::encode(void) {
farbodjam	0:fc0482177a6d	239
farbodjam	0:fc0482177a6d	240	int change = 0;
farbodjam	0:fc0482177a6d	241	int chanA = channelA_.read();
farbodjam	0:fc0482177a6d	242	int chanB = channelB_.read();
farbodjam	0:fc0482177a6d	243
farbodjam	0:fc0482177a6d	244	//2-bit state.
farbodjam	0:fc0482177a6d	245	currState_ = (chanA << 1) \| (chanB);
farbodjam	0:fc0482177a6d	246
farbodjam	0:fc0482177a6d	247	if (encoding_ == X2_ENCODING) {
farbodjam	0:fc0482177a6d	248
farbodjam	0:fc0482177a6d	249	//11->00->11->00 is counter clockwise rotation or "forward".
farbodjam	0:fc0482177a6d	250	if ((prevState_ == 0x3 && currState_ == 0x0) \|\|
farbodjam	0:fc0482177a6d	251	(prevState_ == 0x0 && currState_ == 0x3)) {
farbodjam	0:fc0482177a6d	252
farbodjam	0:fc0482177a6d	253	pulses_++;
farbodjam	0:fc0482177a6d	254
farbodjam	0:fc0482177a6d	255	}
farbodjam	0:fc0482177a6d	256	//10->01->10->01 is clockwise rotation or "backward".
farbodjam	0:fc0482177a6d	257	else if ((prevState_ == 0x2 && currState_ == 0x1) \|\|
farbodjam	0:fc0482177a6d	258	(prevState_ == 0x1 && currState_ == 0x2)) {
farbodjam	0:fc0482177a6d	259
farbodjam	0:fc0482177a6d	260	pulses_--;
farbodjam	0:fc0482177a6d	261
farbodjam	0:fc0482177a6d	262	}
farbodjam	0:fc0482177a6d	263
farbodjam	0:fc0482177a6d	264	} else if (encoding_ == X4_ENCODING) {
farbodjam	0:fc0482177a6d	265
farbodjam	0:fc0482177a6d	266	//Entered a new valid state.
farbodjam	0:fc0482177a6d	267	if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
farbodjam	0:fc0482177a6d	268	//2 bit state. Right hand bit of prev XOR left hand bit of current
farbodjam	0:fc0482177a6d	269	//gives 0 if clockwise rotation and 1 if counter clockwise rotation.
farbodjam	0:fc0482177a6d	270	change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
farbodjam	0:fc0482177a6d	271
farbodjam	0:fc0482177a6d	272	if (change == 0) {
farbodjam	0:fc0482177a6d	273	change = -1;
farbodjam	0:fc0482177a6d	274	}
farbodjam	0:fc0482177a6d	275
farbodjam	0:fc0482177a6d	276	pulses_ -= change;
farbodjam	0:fc0482177a6d	277	}
farbodjam	0:fc0482177a6d	278
farbodjam	0:fc0482177a6d	279	}
farbodjam	0:fc0482177a6d	280
farbodjam	0:fc0482177a6d	281	prevState_ = currState_;
farbodjam	0:fc0482177a6d	282
farbodjam	0:fc0482177a6d	283	}
farbodjam	0:fc0482177a6d	284
farbodjam	0:fc0482177a6d	285	void QEI::index(void) {
farbodjam	0:fc0482177a6d	286
farbodjam	0:fc0482177a6d	287	revolutions_++;
farbodjam	0:fc0482177a6d	288
farbodjam	0:fc0482177a6d	289	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Created:	27 Aug 2011
Imports:	250
Forks:	0
Commits:	1
Dependents:	1
Dependencies:	0
Followers:	4

QEI.cpp@0:fc0482177a6d, 2011-08-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning