encoderKRAI.cpp

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