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 }