QEI.cpp

00001 #include "QEI.h"
00002  
00003 QEI::QEI(PinName channelA,
00004          PinName channelB,
00005          PinName index,
00006          int pulsesPerRev,
00007          Encoding encoding
00008          ) : 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     channelA_.rise(this, &QEI::encode);
00025     channelA_.fall(this, &QEI::encode);
00026  
00027  
00028     if (encoding == X4_ENCODING) {
00029         channelB_.rise(this, &QEI::encode);
00030         channelB_.fall(this, &QEI::encode);
00031     }
00032     if (index !=  NC) {
00033         index_.rise(this, &QEI::index);
00034     }
00035  
00036 }
00037  
00038 void QEI::reset(void) {
00039  
00040     pulses_      = 0;
00041     revolutions_ = 0;
00042     round_rev = 0;
00043     sumangle = angle_ =0;
00044 }
00045 void QEI::set(int pul , int rev) {
00046  
00047     pulses_      = pul;
00048     revolutions_ = rev;
00049  
00050 }
00051 int QEI::getCurrentState(void) {
00052  
00053     return currState_;
00054  
00055 }
00056  
00057 int QEI::getPulses(void) {
00058  
00059     return pulses_;
00060  
00061 }
00062  
00063 int QEI::getRevolutions(void) {
00064  
00065     return revolutions_;
00066  
00067 }
00068 double QEI::getAngle()
00069 {
00070     return angle_;
00071 }
00072 int QEI::getAng_rev()
00073 {
00074     return round_rev;
00075 }
00076 double QEI::getSumangle()
00077 {
00078     return sumangle;
00079 }
00080  
00081 double QEI::getRPM()
00082 {
00083     static double prev_angle;
00084         Mper.stop();
00085         
00086         RPM = (sumangle - prev_angle) / Mper.read() * 60.0 / 360;
00087         Mper.reset();
00088         Mper.start();
00089         prev_angle = sumangle;        
00090     return RPM;
00091 }
00092 double QEI::getRPS()
00093 {
00094     static double prev_angle;
00095         Rper.stop();
00096         
00097         RPS = (sumangle - prev_angle) / Rper.read() / 360;
00098         Rper.reset();
00099         Rper.start();
00100         prev_angle = sumangle;  
00101     return RPS;
00102 }
00103 double QEI::getRPMS()
00104 {    static double prev_angle;
00105         MSper.stop();
00106         
00107         RPMS = (sumangle - prev_angle) / (double)MSper.read_ms() / 360;
00108         MSper.reset();
00109         MSper.start();
00110         prev_angle = sumangle;  
00111     return RPMS;
00112 }
00113 double QEI::getRPUS()
00114 {    static double prev_angle;
00115         USper.stop();
00116         
00117         RPUS = (sumangle - prev_angle) / (double)USper.read_us() / 360;
00118         USper.reset();
00119         USper.start();
00120         prev_angle = sumangle;  
00121     return RPUS;
00122 }
00123 void QEI::encode(void) {
00124  
00125     int change = 0;
00126     int chanA  = channelA_.read();
00127     int chanB  = channelB_.read();
00128  
00129     currState_ = (chanA << 1) | (chanB);
00130     
00131     if (encoding_ == X2_ENCODING) {
00132  
00133         if ((prevState_ == 0x3 && currState_ == 0x0) ||
00134                 (prevState_ == 0x0 && currState_ == 0x3)) {
00135  
00136             pulses_++;
00137             angle_pulses++;
00138  
00139         }
00140         else if ((prevState_ == 0x2 && currState_ == 0x1) ||
00141                  (prevState_ == 0x1 && currState_ == 0x2)) {
00142  
00143             pulses_--;
00144             angle_pulses--;
00145  
00146         }
00147  
00148     } else if (encoding_ == X4_ENCODING) {
00149  
00150         if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
00151             change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
00152  
00153             if (change == 0) {
00154                 change = -1;
00155             }
00156  
00157             pulses_ -= change;
00158             angle_pulses -= change;
00159         }
00160  
00161     }
00162     angle_ = angle_pulses*360/((double)pulsesPerRev_*4);
00163     sumangle = pulses_*360/((double)pulsesPerRev_*4);
00164     if(angle_>=360)
00165     {
00166         angle_pulses = angle_ = 0;
00167         round_rev++;
00168     }
00169     else if(angle_<=-360)
00170     {
00171         angle_pulses = angle_ = 0;
00172         round_rev--;
00173     }
00174     prevState_ = currState_;
00175 }
00176  
00177 void QEI::index(void) {
00178  
00179     revolutions_++;
00180  
00181 }
00182  
00183  
00184