encoderKRAI.cpp

00001 /********************************************************/
00002 /*          Library untuk pembacaan Encoder             */
00003 /*                  Adopsi dari QEI                     */
00004 /*  Bismillahirahmanirrahim                             */
00005 /*  Encoder yang sudah dicoba :                         */
00006 /*  1. Autonics                                         */
00007 /*  2. Encoder bawaan Motor                             */
00008 /*                                                      */
00009 /*  ______________________                              */
00010 /*  |______Autonics______|                              */
00011 /*  | Out A = Input 1    |                              */
00012 /*  | Out B = Input 2    |                              */
00013 /*  | 5V                 |                              */
00014 /*  |_Gnd________________|                              */
00015 /*                                                      */
00016 /********************************************************/
00017 
00018 #include "mbed.h"
00019 #include "encoderKRAI.h"
00020 
00021 encoderKRAI::encoderKRAI(PinName channelA,
00022                          PinName channelB,
00023                          int pulsesPerRev,
00024                          Encoding encoding) : channelA_(channelA), channelB_(channelB)
00025 {
00026     pulses_       = 0;
00027     revolutions_  = 0;
00028     pulsesPerRev_ = pulsesPerRev;
00029     encoding_     = encoding;
00030 
00031     //Workout what the current state is.
00032     int chanA = channelA_.read();
00033     int chanB = channelB_.read();
00034 
00035     //2-bit state.
00036     currState_ = (chanA << 1) | (chanB);
00037     prevState_ = currState_;
00038 
00039     //X2 encoding uses interrupts on only channel A.
00040     //X4 encoding uses interrupts on      channel A,
00041     //and on channel B.
00042     channelA_.rise(this, &encoderKRAI::encode);
00043     channelA_.fall(this, &encoderKRAI::encode);
00044 
00045     //If we're using X4 encoding, then attach interrupts to channel B too.
00046     if (encoding == X4_ENCODING) {
00047         channelB_.rise(this, &encoderKRAI::encode);
00048         channelB_.fall(this, &encoderKRAI::encode);
00049     }
00050 }
00051 
00052 void encoderKRAI::reset(void) {
00053 
00054     pulses_      = 0;
00055     revolutions_ = 0;
00056 
00057 }
00058 
00059 int encoderKRAI::getPulses(void) {
00060 
00061     return pulses_;
00062 
00063 }
00064 
00065 int encoderKRAI::getRevolutions(void) {
00066 
00067     revolutions_ = (float)pulses_ / pulsesPerRev_;
00068     double x = revolutions_ ;
00069     pulses_ = 0;
00070     revolutions_ = 0;    
00071     return x; 
00072 }
00073 
00074 /***************************************
00075  *  Perhitungan Pulse Encoder
00076  ***************************************/
00077 
00078 void encoderKRAI::encode(void) {
00079 
00080     int change = 0;
00081     int chanA  = channelA_.read();
00082     int chanB  = channelB_.read();
00083 
00084     //2-bit state.
00085     currState_ = (chanA << 1) | (chanB);
00086 
00087     if (encoding_ == X2_ENCODING) {
00088 
00089         //11->00->11->00 is counter clockwise rotation or "forward".
00090         if ((prevState_ == 0x3 && currState_ == 0x0) ||
00091                 (prevState_ == 0x0 && currState_ == 0x3)) {
00092 
00093             pulses_++;
00094 
00095         }
00096         //10->01->10->01 is clockwise rotation or "backward".
00097         else if ((prevState_ == 0x2 && currState_ == 0x1) ||
00098                  (prevState_ == 0x1 && currState_ == 0x2)) {
00099 
00100             pulses_--;
00101 
00102         }
00103 
00104     } else if (encoding_ == X4_ENCODING) {
00105 
00106         //Entered a new valid state.
00107         if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
00108             //2 bit state. Right hand bit of prev XOR left hand bit of current
00109             //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
00110             change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
00111 
00112             if (change == 0) {
00113                 change = -1;
00114             }
00115 
00116             pulses_ -= change;
00117         }
00118 
00119     }
00120 
00121     prevState_ = currState_;
00122 
00123 }