This is a library for processing encoder

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers ENCODER_PROCESSOR.cpp Source File

ENCODER_PROCESSOR.cpp

00001 /**
00002  * Note: This is a cross-platform version which is separated from hardware.
00003  * ---------------------------------------------------------------------------
00004  * This module is modified by Chun-Feng Huang for abstracting and more functionality.
00005  * Modified by: Chun-Feng Huang
00006  * E-mail: bens0516@gmail.com
00007  *
00008 */
00009 
00010 //----------------------------------//
00011 /**
00012  * @author Aaron Berk
00013  *
00014  * @section LICENSE
00015  *
00016  * Copyright (c) 2010 ARM Limited
00017  *
00018  * Permission is hereby granted, free of charge, to any person obtaining a copy
00019  * of this software and associated documentation files (the "Software"), to deal
00020  * in the Software without restriction, including without limitation the rights
00021  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00022  * copies of the Software, and to permit persons to whom the Software is
00023  * furnished to do so, subject to the following conditions:
00024  *
00025  * The above copyright notice and this permission notice shall be included in
00026  * all copies or substantial portions of the Software.
00027  *
00028  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00029  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00030  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00031  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00032  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00033  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00034  * THE SOFTWARE.
00035  *
00036  * @section DESCRIPTION
00037  *
00038  * Quadrature Encoder Interface.
00039  *
00040  * A quadrature encoder consists of two code tracks on a disc which are 90
00041  * degrees out of phase. It can be used to determine how far a wheel has
00042  * rotated, relative to a known starting position.
00043  *
00044  * Only one code track changes at a time leading to a more robust system than
00045  * a single track, because any jitter around any edge won't cause a state
00046  * change as the other track will remain constant.
00047  *
00048  * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
00049  * and paper code tracks consisting of alternating black and white sections;
00050  * alternatively, complete disk and PCB emitter/receiver encoder systems can
00051  * be bought, but the interface, regardless of implementation is the same.
00052  *
00053  *               +-----+     +-----+     +-----+
00054  * Channel A     |  ^  |     |     |     |     |
00055  *            ---+  ^  +-----+     +-----+     +-----
00056  *               ^  ^
00057  *               ^  +-----+     +-----+     +-----+
00058  * Channel B     ^  |     |     |     |     |     |
00059  *            ------+     +-----+     +-----+     +-----
00060  *               ^  ^
00061  *               ^  ^
00062  *               90deg
00063  *
00064  * The interface uses X2 encoding by default which calculates the pulse count
00065  * based on reading the current state after each rising and falling edge of
00066  * channel A.
00067  *
00068  *               +-----+     +-----+     +-----+
00069  * Channel A     |     |     |     |     |     |
00070  *            ---+     +-----+     +-----+     +-----
00071  *               ^     ^     ^     ^     ^
00072  *               ^  +-----+  ^  +-----+  ^  +-----+
00073  * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
00074  *            ------+  ^  +-----+  ^  +-----+     +--
00075  *               ^     ^     ^     ^     ^
00076  *               ^     ^     ^     ^     ^
00077  * Pulse count 0 1     2     3     4     5  ...
00078  *
00079  * This interface can also use X4 encoding which calculates the pulse count
00080  * based on reading the current state after each rising and falling edge of
00081  * either channel.
00082  *
00083  *               +-----+     +-----+     +-----+
00084  * Channel A     |     |     |     |     |     |
00085  *            ---+     +-----+     +-----+     +-----
00086  *               ^     ^     ^     ^     ^
00087  *               ^  +-----+  ^  +-----+  ^  +-----+
00088  * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
00089  *            ------+  ^  +-----+  ^  +-----+     +--
00090  *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
00091  *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
00092  * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
00093  *
00094  * It defaults
00095  *
00096  * An optional index channel can be used which determines when a full
00097  * revolution has occured.
00098  *
00099  * If a 4 pules per revolution encoder was used, with X4 encoding,
00100  * the following would be observed.
00101  *
00102  *               +-----+     +-----+     +-----+
00103  * Channel A     |     |     |     |     |     |
00104  *            ---+     +-----+     +-----+     +-----
00105  *               ^     ^     ^     ^     ^
00106  *               ^  +-----+  ^  +-----+  ^  +-----+
00107  * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
00108  *            ------+  ^  +-----+  ^  +-----+     +--
00109  *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
00110  *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
00111  *               ^  ^  ^  +--+  ^  ^  +--+  ^
00112  *               ^  ^  ^  |  |  ^  ^  |  |  ^
00113  * Index      ------------+  +--------+  +-----------
00114  *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
00115  * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
00116  * Rev.  count 0          1           2
00117  *
00118  * Rotational position in degrees can be calculated by:
00119  *
00120  * (pulse count / X * N) * 360
00121  *
00122  * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
00123  * of pulses per revolution.
00124  *
00125  * Linear position can be calculated by:
00126  *
00127  * (pulse count / X * N) * (1 / PPI)
00128  *
00129  * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
00130  * pulses per revolution, and PPI is pulses per inch, or the equivalent for
00131  * any other unit of displacement. PPI can be calculated by taking the
00132  * circumference of the wheel or encoder disk and dividing it by the number
00133  * of pulses per revolution.
00134  */
00135 
00136 /**
00137  * Includes
00138  */
00139 #include "ENCODER_PROCESSOR.h"
00140 
00141 ENCODER_PROCESSOR::ENCODER_PROCESSOR(int pulsesPerRevolution_in, int size_MA_window_in, double sampling_time_in, Encoding encoding):
00142         pulsesPerRevolution(pulsesPerRevolution_in), size_MA_window(size_MA_window_in), Ts(sampling_time_in)
00143 {
00144     //
00145     is_initiated = false;
00146 
00147 
00148     //
00149     MA_window.assign(size_MA_window, 0);
00150 
00151     delta_count = 0;
00152     idx_MA_array = 0;
00153 
00154 
00155     pulses_       = 0;
00156     revolutions_  = 0;
00157     encoding_     = encoding;
00158 
00159     // Findout what the current state is.
00160     // int chanA = channelA_.read();
00161     // int chanB = channelB_.read();
00162 
00163     //2-bit state.
00164     encoderState_now = 0; // (chanA << 1) | (chanB);
00165     encoderState_pre = encoderState_now;
00166 
00167     // Attaching call back function
00168     //X2 encoding uses interrupts on only channel A.
00169     //X4 encoding uses interrupts on      channel A and on channel B.
00170 
00171 
00172     // Unit transformation
00173     // Rotational speed
00174     count_2_rad_s = 1.0/(pulsesPerRevolution*1.0)*6.2831852/Ts/(size_MA_window*1.0);
00175     count_2_deg_s = 1.0/(pulsesPerRevolution*1.0)*360.0/Ts/(size_MA_window*1.0);
00176     // Angle
00177     count_2_rad = 1.0/(pulsesPerRevolution*1.0)*6.2831852;
00178     count_2_deg = 1.0/(pulsesPerRevolution*1.0)*360.0;
00179 }
00180 
00181 // Process control
00182 void ENCODER_PROCESSOR::reset(void) {
00183 
00184     pulses_      = 0;
00185     revolutions_ = 0;
00186 
00187 }
00188 
00189 // Encode (call Back)
00190 /////////////////////////
00191 // +-------------+
00192 // | X2 Encoding |
00193 // +-------------+
00194 //
00195 // When observing states two patterns will appear:
00196 //
00197 // Counter clockwise rotation:
00198 //
00199 // 10 -> 01 -> 10 -> 01 -> ...
00200 //
00201 // Clockwise rotation:
00202 //
00203 // 11 -> 00 -> 11 -> 00 -> ...
00204 //
00205 // We consider counter clockwise rotation to be "forward" and
00206 // counter clockwise to be "backward". Therefore pulse count will increase
00207 // during counter clockwise rotation and decrease during clockwise rotation.
00208 //
00209 // +-------------+
00210 // | X4 Encoding |
00211 // +-------------+
00212 //
00213 // There are four possible states for a quadrature encoder which correspond to
00214 // 2-bit gray code.
00215 //
00216 // A state change is only valid if of only one bit has changed.
00217 // A state change is invalid if both bits have changed.
00218 //
00219 // Clockwise Rotation ->
00220 //  (The bits are constructed as "AB")
00221 //    00 01 11 10 00
00222 //
00223 // <- Counter Clockwise Rotation
00224 //
00225 // If we observe any valid state changes going from left to right, we have
00226 // moved one pulse clockwise [we will consider this "backward" or "negative"].
00227 //
00228 // If we observe any valid state changes going from right to left we have
00229 // moved one pulse counter clockwise [we will consider this "forward" or
00230 // "positive"].
00231 //
00232 // We might enter an invalid state for a number of reasons which are hard to
00233 // predict - if this is the case, it is generally safe to ignore it, update
00234 // the state and carry on, with the error correcting itself shortly after.
00235 
00236 
00237 //---------------------------------------------//
00238 // Call-back function for both pin A and pin B interupt (rise and fall)
00239 void ENCODER_PROCESSOR::IntrCB_pulseUpdate(int phase_A, int phase_B) {
00240 
00241     if (!is_initiated){
00242         //2-bit state.
00243         encoderState_now = (phase_A << 1) | (phase_B); // AB
00244         encoderState_pre = encoderState_now;
00245         //
00246         is_initiated = true;
00247         //
00248         return;
00249     }
00250 
00251     //--------------------------------//
00252     int change = 0;
00253 
00254     //2-bit state.
00255     encoderState_now = (phase_A << 1) | (phase_B); // AB
00256 
00257     if (encoding_ == X2_ENCODING) {
00258 
00259         //11->00->11->00 is counter clockwise rotation or "forward".
00260         if ((encoderState_pre == 0x3 && encoderState_now == 0x0) ||
00261                 (encoderState_pre == 0x0 && encoderState_now == 0x3)) {
00262 
00263             pulses_++;
00264 
00265         }
00266         //10->01->10->01 is clockwise rotation or "backward".
00267         else if ((encoderState_pre == 0x2 && encoderState_now == 0x1) ||
00268                  (encoderState_pre == 0x1 && encoderState_now == 0x2)) {
00269 
00270             pulses_--;
00271 
00272         }
00273 
00274     } else{  // if (encoding_ == X4_ENCODING) {
00275 
00276         //Entered a new valid state.
00277         if (((encoderState_now ^ encoderState_pre) != INVALID) && (encoderState_now != encoderState_pre)) {
00278             // 2 bit state.
00279             // (Right hand bit of prev) XOR (left hand bit of current)
00280             // B_(n-1) xor A_n (because of the 90deg phase shift and A is leading B when rotates clockwise)
00281             // gives 0 if clockwise rotation and 1 if counter clockwise rotation.
00282             change = (encoderState_pre & PREV_MASK) ^ ((encoderState_now & CURR_MASK) >> 1);
00283 
00284             if (change == 0) {
00285                 change = -1;
00286             }
00287 
00288             pulses_ -= change;
00289         }
00290 
00291     }
00292 
00293     encoderState_pre = encoderState_now;
00294 
00295 }
00296 // (Un-necessary) Call-back function for index-pin interupt
00297 void ENCODER_PROCESSOR::IntrCB_indexUpdate(void) {
00298     revolutions_++;
00299 }
00300 //---------------------------------------------//
00301 
00302 
00303 
00304 //---------------------------------------------//
00305 // Iterate at each timer interupt
00306 void ENCODER_PROCESSOR::iterateOnce(){                      // Moving average: calculating the speed of the motor for feedback control
00307     // new input
00308     MA_window[idx_MA_array] = pulses_;
00309 
00310     // idx_next: Actually, it is the oldest one.
00311     size_t idx_next = idx_MA_array + 1;
00312     if(idx_next > (size_MA_window - 1) ){idx_next = 0;}
00313     // Calculate the total number of pulses within the period
00314     delta_count = (MA_window[idx_MA_array] - MA_window[idx_next]);
00315     //
00316     idx_MA_array ++;
00317     if(idx_MA_array > (size_MA_window -1) ){idx_MA_array = 0;}
00318 }
00319 //---------------------------------------------//
00320 
00321 
00322 // Get states
00323 int ENCODER_PROCESSOR::getEncoderState(void) {
00324     return encoderState_now;
00325 }
00326 
00327 int ENCODER_PROCESSOR::getPulses(void) {
00328     return pulses_;
00329 }
00330 
00331 int ENCODER_PROCESSOR::getRevolutions(void) {
00332     return revolutions_;
00333 }
00334 
00335 // Get results
00336 // Rotational speed
00337 double ENCODER_PROCESSOR::getAngularSpeed(){
00338     return delta_count*count_2_rad_s;
00339 }
00340 
00341 double ENCODER_PROCESSOR::getAngularSpeed_deg_s(){
00342     return delta_count*count_2_deg_s;
00343 }
00344 // Angle
00345 double ENCODER_PROCESSOR::getAngle(bool is_ranged){ // rad, if is_ranged, return 0~2*PI
00346     //
00347     int pulse_temp = this->pulses_;
00348     //
00349     if (is_ranged){
00350         revolutions_ = pulse_temp/pulsesPerRevolution;
00351         return (pulse_temp % pulsesPerRevolution)*count_2_rad;
00352     }else{
00353         return (pulse_temp*count_2_rad);
00354     }
00355 }
00356 double ENCODER_PROCESSOR::getAngle_deg(bool is_ranged){ // deg, if is_ranged, return 0~360
00357     //
00358     int pulse_temp = this->pulses_;
00359     //
00360     if (is_ranged){
00361         revolutions_ = pulse_temp/pulsesPerRevolution;
00362         return (pulse_temp % pulsesPerRevolution)*count_2_deg;
00363     }else{
00364         return (pulse_temp*count_2_deg);
00365     }
00366 }