rotary_encoder.cpp

00001 #include "rotary_encoder.h"
00002 
00003 QEI::QEI(PinName channelA,
00004          PinName channelB,
00005          int pulsesPerRev
00006         ) : channelA_(channelA), channelB_(channelB)
00007 {
00008 
00009     pulses_   = 0;
00010     round_rev_d = 0;
00011     round_rev_f = 0;
00012     pulsesPerRev_ = pulsesPerRev;
00013 
00014     //Workout what the current state is.
00015     bool chanA = channelA_.read();
00016     bool chanB = channelB_.read();
00017 
00018     //2-bit state.
00019     currState_ = (chanA << 1) | (chanB);
00020     prevState_ = currState_;
00021 
00022     channelA_.rise(this, &QEI::encode);
00023     channelA_.fall(this, &QEI::encode);
00024 }
00025 
00026 //エンコーダの初期化
00027 void QEI::reset(void)
00028 {
00029     pulses_      = 0;
00030     round_rev_d = 0;
00031     round_rev_f = 0;
00032     sumangle = angle_ =0;
00033 }
00034 
00035 //パルス数,回転回数の再設定(現在地の再設定てきな？)
00036 void QEI::set(int pul, int rev)
00037 {
00038     pulses_      = pul;
00039     round_rev_d = rev;
00040     round_rev_f = rev;
00041 }
00042 
00043 //エンコーダ起動からのパルス数
00044 int QEI::getPulses(void)
00045 {
00046     return pulses_;
00047 }
00048 
00049 //回転回数(int型)
00050 int QEI::getRev_d()
00051 {
00052     return round_rev_d;
00053 }
00054 
00055 //回転回数(float型)
00056 double QEI::getRev_f()
00057 {
00058     return round_rev_f;
00059 }
00060 
00061 //回転角(-360 < angle < 360)
00062 double QEI::getAngle()
00063 {
00064     return angle_;
00065 }
00066 
00067 //今までの回転角
00068 double QEI::getSumangle()
00069 {
00070     return sumangle;
00071 }
00072 
00073 //現在のRPM(rotations per minute：分間回転数)
00074 double QEI::getRPM()
00075 {
00076     static double prev_angle;
00077     Mper.stop();
00078 
00079     RPM = (sumangle - prev_angle) / Mper.read() * 60.0 / 360;
00080     Mper.reset();
00081     Mper.start();
00082     prev_angle = sumangle;
00083     return RPM;
00084 }
00085 
00086 //Interruptするたびに実行される
00087 void QEI::encode(void)
00088 {
00089     bool chanA  = channelA_.read();
00090     bool chanB  = channelB_.read();
00091 
00092     currState_ = (chanA << 1) | (chanB);
00093     if ((prevState_ == 0x3 && currState_ == 0x0) ||
00094             (prevState_ == 0x0 && currState_ == 0x3)) {
00095 
00096         pulses_++;
00097         angle_pulses++;
00098 
00099     } else if ((prevState_ == 0x2 && currState_ == 0x1) ||
00100                (prevState_ == 0x1 && currState_ == 0x2)) {
00101 
00102         pulses_--;
00103         angle_pulses--;
00104 
00105     }
00106     angle_   = angle_pulses *360/   ((double)pulsesPerRev_*2);
00107     sumangle = pulses_      *360/   ((double)pulsesPerRev_*2);
00108     round_rev_f = sumangle / 360;
00109     if(angle_>=360) {
00110         angle_pulses = angle_ = 0;
00111         round_rev_d++;
00112     } else if(angle_<=-360) {
00113         angle_pulses = angle_ = 0;
00114         round_rev_d--;
00115     }
00116     prevState_ = currState_;
00117 }