PullUpMode
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 int pulsesPerRev, 00135 Encoding encoding) : channelA_(channelA), channelB_(channelB), 00136 index_(index) { 00137 00138 pulses_ = 0; 00139 revolutions_ = 0; 00140 pulsesPerRev_ = pulsesPerRev; 00141 encoding_ = encoding; 00142 00143 //Workout what the current state is. 00144 int chanA = channelA_.read(); 00145 int chanB = channelB_.read(); 00146 channelA_.mode(PullUp); 00147 channelB_.mode(PullUp); 00148 //2-bit state. 00149 currState_ = (chanA << 1) | (chanB); 00150 prevState_ = currState_; 00151 00152 //X2 encoding uses interrupts on only channel A. 00153 //X4 encoding uses interrupts on channel A, 00154 //and on channel B. 00155 channelA_.rise(this, &QEI::encode); 00156 channelA_.fall(this, &QEI::encode); 00157 00158 //If we're using X4 encoding, then attach interrupts to channel B too. 00159 if (encoding == X4_ENCODING) { 00160 channelB_.rise(this, &QEI::encode); 00161 channelB_.fall(this, &QEI::encode); 00162 } 00163 //Index is optional. 00164 if (index != NC) { 00165 index_.rise(this, &QEI::index); 00166 } 00167 00168 } 00169 00170 void QEI::reset(void) { 00171 00172 pulses_ = 0; 00173 revolutions_ = 0; 00174 00175 } 00176 00177 int QEI::getCurrentState(void) { 00178 00179 return currState_; 00180 00181 } 00182 00183 int QEI::getPulses(void) { 00184 00185 return pulses_; 00186 00187 } 00188 00189 int QEI::getRevolutions(void) { 00190 00191 return revolutions_; 00192 00193 } 00194 00195 // +-------------+ 00196 // | X2 Encoding | 00197 // +-------------+ 00198 // 00199 // When observing states two patterns will appear: 00200 // 00201 // Counter clockwise rotation: 00202 // 00203 // 10 -> 01 -> 10 -> 01 -> ... 00204 // 00205 // Clockwise rotation: 00206 // 00207 // 11 -> 00 -> 11 -> 00 -> ... 00208 // 00209 // We consider counter clockwise rotation to be "forward" and 00210 // counter clockwise to be "backward". Therefore pulse count will increase 00211 // during counter clockwise rotation and decrease during clockwise rotation. 00212 // 00213 // +-------------+ 00214 // | X4 Encoding | 00215 // +-------------+ 00216 // 00217 // There are four possible states for a quadrature encoder which correspond to 00218 // 2-bit gray code. 00219 // 00220 // A state change is only valid if of only one bit has changed. 00221 // A state change is invalid if both bits have changed. 00222 // 00223 // Clockwise Rotation -> 00224 // 00225 // 00 01 11 10 00 00226 // 00227 // <- Counter Clockwise Rotation 00228 // 00229 // If we observe any valid state changes going from left to right, we have 00230 // moved one pulse clockwise [we will consider this "backward" or "negative"]. 00231 // 00232 // If we observe any valid state changes going from right to left we have 00233 // moved one pulse counter clockwise [we will consider this "forward" or 00234 // "positive"]. 00235 // 00236 // We might enter an invalid state for a number of reasons which are hard to 00237 // predict - if this is the case, it is generally safe to ignore it, update 00238 // the state and carry on, with the error correcting itself shortly after. 00239 void QEI::encode(void) { 00240 00241 int change = 0; 00242 int chanA = channelA_.read(); 00243 int chanB = channelB_.read(); 00244 channelA_.mode(PullDown); 00245 channelB_.mode(PullDown); 00246 //2-bit state. 00247 currState_ = (chanA << 1) | (chanB); 00248 00249 if (encoding_ == X2_ENCODING) { 00250 00251 //11->00->11->00 is counter clockwise rotation or "forward". 00252 if ((prevState_ == 0x3 && currState_ == 0x0) || 00253 (prevState_ == 0x0 && currState_ == 0x3)) { 00254 00255 pulses_++; 00256 00257 } 00258 //10->01->10->01 is clockwise rotation or "backward". 00259 else if ((prevState_ == 0x2 && currState_ == 0x1) || 00260 (prevState_ == 0x1 && currState_ == 0x2)) { 00261 00262 pulses_--; 00263 00264 } 00265 00266 } else if (encoding_ == X4_ENCODING) { 00267 00268 //Entered a new valid state. 00269 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { 00270 //2 bit state. Right hand bit of prev XOR left hand bit of current 00271 //gives 0 if clockwise rotation and 1 if counter clockwise rotation. 00272 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); 00273 00274 if (change == 0) { 00275 change = -1; 00276 } 00277 00278 pulses_ -= change; 00279 } 00280 00281 } 00282 00283 prevState_ = currState_; 00284 00285 } 00286 00287 void QEI::index(void) { 00288 00289 revolutions_++; 00290 00291 }
Generated on Sun Jul 17 2022 17:31:55 by 1.7.2