Patrick Wensing / QEI_pmw

QEI library with velocity implementation based on time difference between successive encoder counts.

Dependents:   Bezier_Trajectory_Follower Dolphin 2_131TEST Jerby ... more

QEI.cpp@4:58a3c9c9e956, 2015-09-23 (annotated)

Committer:
pwensing
Date:
Wed Sep 23 17:33:09 2015 +0000
Revision:
4:58a3c9c9e956
Parent:
0:43b9eeecccc7 
trying volatiles for smoother velocity

Who changed what in which revision?

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