Quadrature encoder interface library with distance function
Dependents: QEI_with_distance wheelchaircontrol wheelchaircontrolRos wheelchaircontrol ... more
Fork of QEI by
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 ti.start(); 00144 //Workout what the current state is. 00145 int chanA = channelA_.read(); 00146 int chanB = channelB_.read(); 00147 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 00190 // +-------------+ 00191 // | X2 Encoding | 00192 // +-------------+ 00193 // 00194 // When observing states two patterns will appear: 00195 // 00196 // Counter clockwise rotation: 00197 // 00198 // 10 -> 01 -> 10 -> 01 -> ... 00199 // 00200 // Clockwise rotation: 00201 // 00202 // 11 -> 00 -> 11 -> 00 -> ... 00203 // 00204 // We consider counter clockwise rotation to be "forward" and 00205 // counter clockwise to be "backward". Therefore pulse count will increase 00206 // during counter clockwise rotation and decrease during clockwise rotation. 00207 // 00208 // +-------------+ 00209 // | X4 Encoding | 00210 // +-------------+ 00211 // 00212 // There are four possible states for a quadrature encoder which correspond to 00213 // 2-bit gray code. 00214 // 00215 // A state change is only valid if of only one bit has changed. 00216 // A state change is invalid if both bits have changed. 00217 // 00218 // Clockwise Rotation -> 00219 // 00220 // 00 01 11 10 00 00221 // 00222 // <- Counter Clockwise Rotation 00223 // 00224 // If we observe any valid state changes going from left to right, we have 00225 // moved one pulse clockwise [we will consider this "backward" or "negative"]. 00226 // 00227 // If we observe any valid state changes going from right to left we have 00228 // moved one pulse counter clockwise [we will consider this "forward" or 00229 // "positive"]. 00230 // 00231 // We might enter an invalid state for a number of reasons which are hard to 00232 // predict - if this is the case, it is generally safe to ignore it, update 00233 // the state and carry on, with the error correcting itself shortly after. 00234 void QEI::encode(void) { 00235 00236 int change = 0; 00237 int chanA = channelA_.read(); 00238 int chanB = channelB_.read(); 00239 // printf("in here\r\n"); 00240 //2-bit state. 00241 currState_ = (chanA << 1) | (chanB); 00242 00243 if (encoding_ == X2_ENCODING) { 00244 00245 //11->00->11->00 is counter clockwise rotation or "forward". 00246 if ((prevState_ == 0x3 && currState_ == 0x0) || 00247 (prevState_ == 0x0 && currState_ == 0x3)) { 00248 00249 pulses_++; 00250 00251 } 00252 //10->01->10->01 is clockwise rotation or "backward". 00253 else if ((prevState_ == 0x2 && currState_ == 0x1) || 00254 (prevState_ == 0x1 && currState_ == 0x2)) { 00255 00256 pulses_--; 00257 00258 } 00259 00260 } else if (encoding_ == X4_ENCODING) { 00261 00262 //Entered a new valid state. 00263 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { 00264 //2 bit state. Right hand bit of prev XOR left hand bit of current 00265 //gives 0 if clockwise rotation and 1 if counter clockwise rotation. 00266 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); 00267 00268 if (change == 0) { 00269 change = -1; 00270 } 00271 00272 pulses_ -= change; 00273 } 00274 } 00275 prevState_ = currState_; 00276 } 00277 00278 float QEI::getRevolutions(){ 00279 if (encoding_ == X2_ENCODING) 00280 { 00281 return (float)pulses_ / pulsesPerRev_; 00282 } 00283 else 00284 return (float)pulses_ / (4 * pulsesPerRev_); 00285 } 00286 00287 float QEI::getDistance(float diameter) 00288 { 00289 oldDistance_ = newDistance_; 00290 newDistance_ = getRevolutions()*diameter*3.1415926; 00291 return newDistance_; 00292 } 00293 00294 void QEI::index(void) { 00295 00296 revolutions_++; 00297 } 00298 float QEI::getVelosity() 00299 { 00300 getDistance(53.975); 00301 velosity_ = ti.read()*(newDistance_-oldDistance_); 00302 ti.reset(); 00303 return velosity_; 00304 } 00305
Generated on Wed Jul 13 2022 07:08:19 by 1.7.2