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