Rotork Research Team / QEI

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of QEI by Simon Truelove

QEI.cpp@1:0925301c3257, 2018-11-14 (annotated)

Committer:
simontruelove
Date:
Wed Nov 14 09:57:55 2018 +0000
Revision:
1:0925301c3257
Parent:
0:5c2ad81551aa
Child:
2:5bdf87e1ca68
1

Who changed what in which revision?

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