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