New QEI library with position (angle) outputs
Fork of QEI_modified 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 int arraysize, 00136 double sampletime, 00137 Encoding encoding) : channelA_(channelA), channelB_(channelB), 00138 index_(index) { 00139 00140 sampletime_ = sampletime; 00141 arraysize_ = arraysize; 00142 countArray = new int[arraysize_]; 00143 angularvelocity = 0; 00144 arrayPtr = 0; 00145 for(int i = 0; i < arraysize_; i++){ 00146 countArray[i] = 0; 00147 } 00148 00149 pulses_ = 0; 00150 revolutions_ = 0; 00151 pulsesPerRev_ = pulsesPerRev; 00152 encoding_ = encoding; 00153 00154 //Workout what the current state is. 00155 int chanA = channelA_.read(); 00156 int chanB = channelB_.read(); 00157 00158 //2-bit state. 00159 currState_ = (chanA << 1) | (chanB); 00160 prevState_ = currState_; 00161 00162 // Attaching call back function 00163 //X2 encoding uses interrupts on only channel A. 00164 //X4 encoding uses interrupts on channel A and on channel B. 00165 channelA_.rise(this, &QEI::encode); 00166 channelA_.fall(this, &QEI::encode); 00167 00168 //If we're using X4 encoding, then attach interrupts to channel B too. 00169 if (encoding == X4_ENCODING) { 00170 channelB_.rise(this, &QEI::encode); 00171 channelB_.fall(this, &QEI::encode); 00172 } 00173 //Index is optional. 00174 if (index != NC) { 00175 index_.rise(this, &QEI::index); 00176 } 00177 00178 // Unit transformation 00179 // Rotational speed 00180 count_2_rad_s = 1.0/(pulsesPerRev_*1.0)*6.2831852/sampletime_/(arraysize_*1.0); 00181 count_2_deg_s = 1.0/(pulsesPerRev_*1.0)*360.0/sampletime_/(arraysize_*1.0); 00182 // Angle 00183 count_2_rad = 1.0/(pulsesPerRev_*1.0)*6.2831852; 00184 count_2_deg = 1.0/(pulsesPerRev_*1.0)*360.0; 00185 } 00186 00187 void QEI::reset(void) { 00188 00189 pulses_ = 0; 00190 revolutions_ = 0; 00191 00192 } 00193 00194 int QEI::getCurrentState(void) { 00195 00196 return currState_; 00197 00198 } 00199 00200 int QEI::getPulses(void) { 00201 00202 return pulses_; 00203 00204 } 00205 00206 int QEI::getRevolutions(void) { 00207 00208 return revolutions_; 00209 00210 } 00211 00212 // Encode (call Back) 00213 ///////////////////////// 00214 // +-------------+ 00215 // | X2 Encoding | 00216 // +-------------+ 00217 // 00218 // When observing states two patterns will appear: 00219 // 00220 // Counter clockwise rotation: 00221 // 00222 // 10 -> 01 -> 10 -> 01 -> ... 00223 // 00224 // Clockwise rotation: 00225 // 00226 // 11 -> 00 -> 11 -> 00 -> ... 00227 // 00228 // We consider counter clockwise rotation to be "forward" and 00229 // counter clockwise to be "backward". Therefore pulse count will increase 00230 // during counter clockwise rotation and decrease during clockwise rotation. 00231 // 00232 // +-------------+ 00233 // | X4 Encoding | 00234 // +-------------+ 00235 // 00236 // There are four possible states for a quadrature encoder which correspond to 00237 // 2-bit gray code. 00238 // 00239 // A state change is only valid if of only one bit has changed. 00240 // A state change is invalid if both bits have changed. 00241 // 00242 // Clockwise Rotation -> 00243 // (The bits are constructed as "AB") 00244 // 00 01 11 10 00 00245 // 00246 // <- Counter Clockwise Rotation 00247 // 00248 // If we observe any valid state changes going from left to right, we have 00249 // moved one pulse clockwise [we will consider this "backward" or "negative"]. 00250 // 00251 // If we observe any valid state changes going from right to left we have 00252 // moved one pulse counter clockwise [we will consider this "forward" or 00253 // "positive"]. 00254 // 00255 // We might enter an invalid state for a number of reasons which are hard to 00256 // predict - if this is the case, it is generally safe to ignore it, update 00257 // the state and carry on, with the error correcting itself shortly after. 00258 void QEI::encode(void) { 00259 00260 int change = 0; 00261 int chanA = channelA_.read(); 00262 int chanB = channelB_.read(); 00263 00264 //2-bit state. 00265 currState_ = (chanA << 1) | (chanB); // AB 00266 00267 if (encoding_ == X2_ENCODING) { 00268 00269 //11->00->11->00 is counter clockwise rotation or "forward". 00270 if ((prevState_ == 0x3 && currState_ == 0x0) || 00271 (prevState_ == 0x0 && currState_ == 0x3)) { 00272 00273 pulses_++; 00274 00275 } 00276 //10->01->10->01 is clockwise rotation or "backward". 00277 else if ((prevState_ == 0x2 && currState_ == 0x1) || 00278 (prevState_ == 0x1 && currState_ == 0x2)) { 00279 00280 pulses_--; 00281 00282 } 00283 00284 } else if (encoding_ == X4_ENCODING) { 00285 00286 //Entered a new valid state. 00287 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { 00288 // 2 bit state. 00289 // (Right hand bit of prev) XOR (left hand bit of current) 00290 // B_(n-1) xor A_n (because of the 90deg phase shift and A is leading B when rotates clockwise) 00291 // gives 0 if clockwise rotation and 1 if counter clockwise rotation. 00292 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); 00293 00294 if (change == 0) { 00295 change = -1; 00296 } 00297 00298 pulses_ -= change; 00299 } 00300 00301 } 00302 00303 prevState_ = currState_; 00304 00305 } 00306 ///////////////////////// end Encode (call Back) 00307 00308 void QEI::index(void) { 00309 00310 revolutions_++; 00311 00312 } 00313 00314 void QEI::Calculate(){ // Moving average: calculating the speed of the motor for feedback control 00315 // new input 00316 countArray[arrayPtr] = pulses_; 00317 00318 // nextPtr: Actually, it is the oldest one. 00319 uint8_t nextPtr = arrayPtr + 1; 00320 if(nextPtr > (arraysize_ - 1) ){nextPtr = 0;} 00321 // Calculate the total number of pulses within the period 00322 angularvelocity = (countArray[arrayPtr] - countArray[nextPtr]); 00323 // 00324 arrayPtr ++; 00325 if(arrayPtr > (arraysize_ -1) ){arrayPtr = 0;} 00326 } 00327 00328 double QEI::getAngularSpeed(){ 00329 return angularvelocity*count_2_rad_s; 00330 } 00331 00332 double QEI::getAngularSpeed_deg_s(){ 00333 return angularvelocity*count_2_deg_s; 00334 } 00335 // Angle 00336 double QEI::getAngle(bool is_ranged){ // rad, if is_ranged, return 0~2*PI 00337 // 00338 int pulse_temp = this->pulses_; 00339 // 00340 if (is_ranged){ 00341 revolutions_ = pulse_temp/pulsesPerRev_; 00342 return (pulse_temp % pulsesPerRev_)*count_2_rad; 00343 }else{ 00344 return (pulse_temp*count_2_rad); 00345 } 00346 } 00347 double QEI::getAngle_deg(bool is_ranged){ // deg, if is_ranged, return 0~360 00348 // 00349 int pulse_temp = this->pulses_; 00350 // 00351 if (is_ranged){ 00352 revolutions_ = pulse_temp/pulsesPerRev_; 00353 return (pulse_temp % pulsesPerRev_)*count_2_deg; 00354 }else{ 00355 return (pulse_temp*count_2_deg); 00356 } 00357 }
Generated on Tue Aug 2 2022 01:49:33 by
1.7.2
