Quadrature encoder interface library.
Fork of QEI by
Embed:
(wiki syntax)
Show/hide line numbers
QEI.cpp
00001 /** 00002 * @author Aaron Berk 00003 * 00004 * @section LICENSE 00005 * 00006 * Copyright (c) 2010 ARM Limited 00007 * 00008 * Permission is hereby granted, free of charge, to any person obtaining a copy 00009 * of this software and associated documentation files (the "Software"), to deal 00010 * in the Software without restriction, including without limitation the rights 00011 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00012 * copies of the Software, and to permit persons to whom the Software is 00013 * furnished to do so, subject to the following conditions: 00014 * 00015 * The above copyright notice and this permission notice shall be included in 00016 * all copies or substantial portions of the Software. 00017 * 00018 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00019 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00020 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00021 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00022 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00023 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00024 * THE SOFTWARE. 00025 * 00026 * @section DESCRIPTION 00027 * 00028 * Quadrature Encoder Interface. 00029 * 00030 * A quadrature encoder consists of two code tracks on a disc which are 90 00031 * degrees out of phase. It can be used to determine how far a wheel has 00032 * rotated, relative to a known starting position. 00033 * 00034 * Only one code track changes at a time leading to a more robust system than 00035 * a single track, because any jitter around any edge won't cause a state 00036 * change as the other track will remain constant. 00037 * 00038 * Encoders can be a homebrew affair, consisting of infrared emitters/receivers 00039 * and paper code tracks consisting of alternating black and white sections; 00040 * alternatively, complete disk and PCB emitter/receiver encoder systems can 00041 * be bought, but the interface, regardless of implementation is the same. 00042 * 00043 * +-----+ +-----+ +-----+ 00044 * Channel A | ^ | | | | | 00045 * ---+ ^ +-----+ +-----+ +----- 00046 * ^ ^ 00047 * ^ +-----+ +-----+ +-----+ 00048 * Channel B ^ | | | | | | 00049 * ------+ +-----+ +-----+ +----- 00050 * ^ ^ 00051 * ^ ^ 00052 * 90deg 00053 * 00054 * The interface uses X2 encoding by default which calculates the pulse count 00055 * based on reading the current state after each rising and falling edge of 00056 * channel A. 00057 * 00058 * +-----+ +-----+ +-----+ 00059 * Channel A | | | | | | 00060 * ---+ +-----+ +-----+ +----- 00061 * ^ ^ ^ ^ ^ 00062 * ^ +-----+ ^ +-----+ ^ +-----+ 00063 * Channel B ^ | ^ | ^ | ^ | ^ | | 00064 * ------+ ^ +-----+ ^ +-----+ +-- 00065 * ^ ^ ^ ^ ^ 00066 * ^ ^ ^ ^ ^ 00067 * Pulse count 0 1 2 3 4 5 ... 00068 * 00069 * This interface can also use X4 encoding which calculates the pulse count 00070 * based on reading the current state after each rising and falling edge of 00071 * either channel. 00072 * 00073 * +-----+ +-----+ +-----+ 00074 * Channel A | | | | | | 00075 * ---+ +-----+ +-----+ +----- 00076 * ^ ^ ^ ^ ^ 00077 * ^ +-----+ ^ +-----+ ^ +-----+ 00078 * Channel B ^ | ^ | ^ | ^ | ^ | | 00079 * ------+ ^ +-----+ ^ +-----+ +-- 00080 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ 00081 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ 00082 * Pulse count 0 1 2 3 4 5 6 7 8 9 ... 00083 * 00084 * It defaults 00085 * 00086 * An optional index channel can be used which determines when a full 00087 * revolution has occured. 00088 * 00089 * If a 4 pules per revolution encoder was used, with X4 encoding, 00090 * the following would be observed. 00091 * 00092 * +-----+ +-----+ +-----+ 00093 * Channel A | | | | | | 00094 * ---+ +-----+ +-----+ +----- 00095 * ^ ^ ^ ^ ^ 00096 * ^ +-----+ ^ +-----+ ^ +-----+ 00097 * Channel B ^ | ^ | ^ | ^ | ^ | | 00098 * ------+ ^ +-----+ ^ +-----+ +-- 00099 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ 00100 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ 00101 * ^ ^ ^ +--+ ^ ^ +--+ ^ 00102 * ^ ^ ^ | | ^ ^ | | ^ 00103 * Index ------------+ +--------+ +----------- 00104 * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ 00105 * Pulse count 0 1 2 3 4 5 6 7 8 9 ... 00106 * Rev. count 0 1 2 00107 * 00108 * Rotational position in degrees can be calculated by: 00109 * 00110 * (pulse count / X * N) * 360 00111 * 00112 * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number 00113 * of pulses per revolution. 00114 * 00115 * Linear position can be calculated by: 00116 * 00117 * (pulse count / X * N) * (1 / PPI) 00118 * 00119 * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of 00120 * pulses per revolution, and PPI is pulses per inch, or the equivalent for 00121 * any other unit of displacement. PPI can be calculated by taking the 00122 * circumference of the wheel or encoder disk and dividing it by the number 00123 * of pulses per revolution. 00124 */ 00125 00126 /** 00127 * Includes 00128 */ 00129 #include "QEI.h" 00130 00131 QEI::QEI(PinName channelA, 00132 PinName channelB, 00133 PinName index, 00134 PinMode intRes, 00135 int pulsesPerRev, 00136 Encoding encoding) : channelA_(channelA), channelB_(channelB), 00137 index_(index) { 00138 00139 00140 pulses_ = 0; 00141 revolutions_ = 0; 00142 pulsesPerRev_ = pulsesPerRev; 00143 encoding_ = encoding; 00144 channelA_.mode(intRes); 00145 channelB_.mode(intRes); 00146 00147 //Workout what the current state is. 00148 int chanA = channelA_.read(); 00149 int chanB = channelB_.read(); 00150 00151 //2-bit state. 00152 currState_ = (chanA << 1) | (chanB); 00153 prevState_ = currState_; 00154 00155 //X2 encoding uses interrupts on only channel A. 00156 //X4 encoding uses interrupts on channel A, 00157 //and on channel B. 00158 channelA_.rise(this, &QEI::encode); 00159 channelA_.fall(this, &QEI::encode); 00160 00161 //If we're using X4 encoding, then attach interrupts to channel B too. 00162 if (encoding == X4_ENCODING) { 00163 channelB_.rise(this, &QEI::encode); 00164 channelB_.fall(this, &QEI::encode); 00165 } 00166 //Index is optional. 00167 if (index != NC) { 00168 index_.rise(this, &QEI::index); 00169 } 00170 00171 } 00172 00173 void QEI::reset(void) { 00174 00175 pulses_ = 0; 00176 revolutions_ = 0; 00177 00178 } 00179 00180 int QEI::getCurrentState(void) { 00181 00182 return currState_; 00183 00184 } 00185 00186 int QEI::getPulses(void) { 00187 00188 return pulses_; 00189 00190 } 00191 00192 int QEI::getRevolutions(void) { 00193 00194 return revolutions_; 00195 00196 } 00197 00198 // +-------------+ 00199 // | X2 Encoding | 00200 // +-------------+ 00201 // 00202 // When observing states two patterns will appear: 00203 // 00204 // Counter clockwise rotation: 00205 // 00206 // 10 -> 01 -> 10 -> 01 -> ... 00207 // 00208 // Clockwise rotation: 00209 // 00210 // 11 -> 00 -> 11 -> 00 -> ... 00211 // 00212 // We consider counter clockwise rotation to be "forward" and 00213 // counter clockwise to be "backward". Therefore pulse count will increase 00214 // during counter clockwise rotation and decrease during clockwise rotation. 00215 // 00216 // +-------------+ 00217 // | X4 Encoding | 00218 // +-------------+ 00219 // 00220 // There are four possible states for a quadrature encoder which correspond to 00221 // 2-bit gray code. 00222 // 00223 // A state change is only valid if of only one bit has changed. 00224 // A state change is invalid if both bits have changed. 00225 // 00226 // Clockwise Rotation -> 00227 // 00228 // 00 01 11 10 00 00229 // 00230 // <- Counter Clockwise Rotation 00231 // 00232 // If we observe any valid state changes going from left to right, we have 00233 // moved one pulse clockwise [we will consider this "backward" or "negative"]. 00234 // 00235 // If we observe any valid state changes going from right to left we have 00236 // moved one pulse counter clockwise [we will consider this "forward" or 00237 // "positive"]. 00238 // 00239 // We might enter an invalid state for a number of reasons which are hard to 00240 // predict - if this is the case, it is generally safe to ignore it, update 00241 // the state and carry on, with the error correcting itself shortly after. 00242 void QEI::encode(void) { 00243 00244 int change = 0; 00245 int chanA = channelA_.read(); 00246 int chanB = channelB_.read(); 00247 00248 //2-bit state. 00249 currState_ = (chanA << 1) | (chanB); 00250 00251 if (encoding_ == X2_ENCODING) { 00252 00253 //11->00->11->00 is counter clockwise rotation or "forward". 00254 if ((prevState_ == 0x3 && currState_ == 0x0) || 00255 (prevState_ == 0x0 && currState_ == 0x3)) { 00256 00257 pulses_++; 00258 00259 } 00260 //10->01->10->01 is clockwise rotation or "backward". 00261 else if ((prevState_ == 0x2 && currState_ == 0x1) || 00262 (prevState_ == 0x1 && currState_ == 0x2)) { 00263 00264 pulses_--; 00265 00266 } 00267 00268 } else if (encoding_ == X4_ENCODING) { 00269 00270 //Entered a new valid state. 00271 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { 00272 //2 bit state. Right hand bit of prev XOR left hand bit of current 00273 //gives 0 if clockwise rotation and 1 if counter clockwise rotation. 00274 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); 00275 00276 if (change == 0) { 00277 change = -1; 00278 } 00279 00280 pulses_ -= change; 00281 } 00282 00283 } 00284 00285 prevState_ = currState_; 00286 00287 } 00288 00289 void QEI::index(void) { 00290 00291 revolutions_++; 00292 00293 }
Generated on Sun Jul 17 2022 17:31:30 by 1.7.2