QEI.cpp

00001 #include "QEI.h"
00002 QEI::QEI(const QEI& q) : channelA_(q.Pin[0]), channelB_(q.Pin[1]),index_(q.Pin[2])
00003 {
00004     pulses_       = 0;
00005     revolutions_  = 0;
00006     pulsesPerRev_ = q.pulsesPerRev_;
00007     encoding_     = q.encoding_;
00008     timer=q.timer;
00009     //Workout what the current state is.
00010     int chanA = channelA_.read();
00011     int chanB = channelB_.read();
00012 
00013     //2-bit state.
00014     currState_ = (chanA << 1) | (chanB);
00015     prevState_ = currState_;
00016 
00017     channelA_.rise(this, &QEI::encode);
00018     channelA_.fall(this, &QEI::encode);
00019 
00020 
00021     if (q.encoding_ == X4_ENCODING) {
00022         channelB_.rise(this, &QEI::encode);
00023         channelB_.fall(this, &QEI::encode);
00024     }
00025     if (Pin[2] !=  NC) {
00026         index_.rise(this, &QEI::index);
00027     }
00028 }
00029 QEI::QEI(PinName channelA,
00030          PinName channelB,
00031          PinName index,
00032          int pulsesPerRev,
00033          Timer *T,
00034          Encoding encoding
00035         ) : channelA_(channelA), channelB_(channelB),
00036     index_(index)
00037 {
00038     timer=T;
00039     Pin[0] = channelA;
00040     Pin[1] = channelB;
00041     Pin[2] = index;
00042     pulses_       = 0;
00043     revolutions_  = 0;
00044     pulsesPerRev_ = pulsesPerRev;
00045     encoding_     = encoding;
00046 
00047     //Workout what the current state is.
00048     int chanA = channelA_.read();
00049     int chanB = channelB_.read();
00050 
00051     //2-bit state.
00052     currState_ = (chanA << 1) | (chanB);
00053     prevState_ = currState_;
00054 
00055     channelA_.rise(this, &QEI::encode);
00056     channelA_.fall(this, &QEI::encode);
00057 
00058 
00059     if (encoding == X4_ENCODING) {
00060         channelB_.rise(this, &QEI::encode);
00061         channelB_.fall(this, &QEI::encode);
00062     }
00063     if (index !=  NC) {
00064         index_.rise(this, &QEI::index);
00065     }
00066 
00067 }
00068 void QEI::state(int i)
00069 {
00070     if(i==1) {
00071         channelA_.disable_irq();
00072         channelB_.disable_irq();
00073     } else if(i==0) {
00074         channelA_.enable_irq();
00075         channelB_.enable_irq();
00076     }
00077 }
00078 void QEI::qei_reset(void)
00079 {
00080 
00081     pulses_      = 0;
00082     revolutions_ = 0;
00083     round_rev = 0;
00084     sumangle = angle_ =0;
00085 }
00086 void QEI::set(int pul , int rev)
00087 {
00088 
00089     pulses_      = pul;
00090     revolutions_ = rev;
00091 
00092 }
00093 int QEI::getCurrentState(void)
00094 {
00095 
00096     return currState_;
00097 
00098 }
00099 
00100 int QEI::getPulses(void)
00101 {
00102 
00103     return pulses_;
00104 
00105 }
00106 
00107 int QEI::getRevolutions(void)
00108 {
00109 
00110     return revolutions_;
00111 
00112 }
00113 double QEI::getAngle()
00114 {
00115     return angle_;
00116 }
00117 int QEI::getAng_rev()
00118 {
00119     return round_rev;
00120 }
00121 double QEI::getSumangle()
00122 {
00123     return sumangle;
00124 }
00125 
00126 double QEI::getRPM()
00127 {
00128     static double prev_angle;
00129 
00130     RPM = (sumangle - prev_angle) / gettime() * 60.0 / 360;
00131 
00132     prev_angle = sumangle;
00133     return RPM;
00134 }
00135 double QEI::getRPS()
00136 {
00137     static double prev_angle;
00138 
00139     RPS = (sumangle - prev_angle) / gettime() / 360;
00140     prev_angle = sumangle;
00141     return RPS;
00142 }
00143 double QEI::getRPMS()
00144 {
00145     static double prev_angle;
00146 
00147     RPMS = (sumangle - prev_angle) / gettime()*1000/ 360;
00148     prev_angle = sumangle;
00149     return RPMS;
00150 }
00151 double QEI::getRPUS()
00152 {
00153     static double prev_angle;
00154     RPUS =(sumangle - prev_angle) / gettime()*1000*1000/ 360;
00155     prev_angle = sumangle;
00156     prev_angle = sumangle;
00157     return RPUS;
00158 }
00159 void QEI::encode(void)
00160 {
00161     int change = 0;
00162     int chanA  = channelA_.read();
00163     int chanB  = channelB_.read();
00164     //printf("QEI\n");
00165     currState_ = (chanA << 1) | (chanB);
00166 
00167     if (encoding_ == X2_ENCODING) {
00168 
00169         if ((prevState_ == 0x3 && currState_ == 0x0) ||
00170                 (prevState_ == 0x0 && currState_ == 0x3)) {
00171 
00172             pulses_++;
00173             angle_pulses++;
00174 
00175         } else if ((prevState_ == 0x2 && currState_ == 0x1) ||
00176                    (prevState_ == 0x1 && currState_ == 0x2)) {
00177 
00178             pulses_--;
00179             angle_pulses--;
00180 
00181         }
00182 
00183     } else if (encoding_ == X4_ENCODING) {
00184 
00185         if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
00186             change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
00187 
00188             if (change == 0) {
00189                 change = -1;
00190             }
00191 
00192             pulses_ -= change;
00193             angle_pulses -= change;
00194         }
00195 
00196     }
00197     angle_ = angle_pulses*360/((double)pulsesPerRev_*4);
00198     sumangle = pulses_*360/((double)pulsesPerRev_*4);
00199     if(angle_>=360) {
00200         angle_pulses = angle_ = 0;
00201         round_rev++;
00202     } else if(angle_<=-360) {
00203         angle_pulses = angle_ = 0;
00204         round_rev--;
00205     }
00206     prevState_ = currState_;
00207 }
00208 
00209 void QEI::index(void)
00210 {
00211 
00212     revolutions_++;
00213 
00214 }
00215