Buggy bois / Mbed 2 deprecated UDITTEST

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.

Dependencies:   mbed

QEI/QEI.cpp@10:ccf6155e3f97, 2019-04-28 (annotated)

Committer:
mazdo25
Date:
Sun Apr 28 20:50:35 2019 +0000
Revision:
10:ccf6155e3f97
Parent:
3:01b5e80d842d 
TRY 1: Udit

Who changed what in which revision?

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