QEI.cpp

00001 
00002 #include "QEI.h"
00003 
00004 QEI::QEI(PinName channelA,
00005          PinName channelB,
00006          PinName index,
00007          int pulsesPerRev,
00008          Encoding encoding) : channelA_(channelA), channelB_(channelB),
00009         index_(index) {
00010 
00011     pulses_       = 0;
00012     revolutions_  = 0;
00013     pulsesPerRev_ = pulsesPerRev;
00014     encoding_     = encoding;
00015 
00016     //Workout what the current state is.
00017     int chanA = channelA_.read();
00018     int chanB = channelB_.read();
00019 
00020     //2-bit state.
00021     currState_ = (chanA << 1) | (chanB);
00022     prevState_ = currState_;
00023 
00024     //X2 encoding uses interrupts on only channel A.
00025     //X4 encoding uses interrupts on      channel A,
00026     //and on channel B.
00027     channelA_.rise(this, &QEI::encode);
00028     channelA_.fall(this, &QEI::encode);
00029 
00030     //If we're using X4 encoding, then attach interrupts to channel B too.
00031     if (encoding == X4_ENCODING) {
00032         channelB_.rise(this, &QEI::encode);
00033         channelB_.fall(this, &QEI::encode);
00034     }
00035     //Index is optional.
00036     if (index !=  NC) {
00037         index_.rise(this, &QEI::index);
00038     }
00039 }
00040 
00041 void QEI::reset(void) {
00042     pulses_      = 0;
00043     revolutions_ = 0;
00044 }
00045 
00046 int QEI::getCurrentState(void) {
00047     return currState_;
00048 }
00049 
00050 int QEI::getPulses(void) {
00051     return pulses_;
00052 }
00053 
00054 int QEI::getRevolutions(void) {
00055     return revolutions_;
00056 }
00057 
00058 void QEI::encode(void) {
00059     int change = 0;
00060     int chanA  = channelA_.read();
00061     int chanB  = channelB_.read();
00062     //2-bit state.
00063     currState_ = (chanA << 1) | (chanB);
00064 
00065     if (encoding_ == X2_ENCODING) {
00066         //11->00->11->00 is counter clockwise rotation or "forward".
00067         if ((prevState_ == 0x3 && currState_ == 0x0) ||
00068                 (prevState_ == 0x0 && currState_ == 0x3)) {
00069             pulses_++;
00070         }
00071         //10->01->10->01 is clockwise rotation or "backward".
00072         else if ((prevState_ == 0x2 && currState_ == 0x1) ||
00073                  (prevState_ == 0x1 && currState_ == 0x2)) {
00074             pulses_--;
00075         }
00076     } else if (encoding_ == X4_ENCODING) {
00077         //Entered a new valid state.
00078         if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
00079             //2 bit state. Right hand bit of prev XOR left hand bit of current
00080             //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
00081             change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
00082             if (change == 0) {
00083                 change = -1;
00084             }
00085             pulses_ -= change;
00086         }
00087     }
00088     prevState_ = currState_;
00089 }
00090 
00091 void QEI::index(void) {
00092     revolutions_++;
00093 }
00094 /*
00095 void QEI::move(float theta_z) {
00096     float tmp = theta_z.getPulses();
00097     return tmp;
00098 }
00099 */