自分用QEI

Dependents:   FourOmniMecha

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers QEI.cpp Source File

QEI.cpp

00001 #include "QEI.h"
00002 #define PI 3.141592653589793
00003 
00004 QEI::QEI(PinName A,PinName B,int PPR,Timer *timer,Ticker *ticker):chA(A),chB(B)
00005 {
00006     _timer = timer;
00007     _ticker = ticker;
00008     pre_time=0;      //修正1:修正による初期化
00009     _PPR = PPR;
00010     init();
00011 }
00012 
00013 void QEI::init()
00014 {
00015     for(int i =0; i<data_sizeof; i++) pulseData[i] = 0;
00016     NrotDIR = 1;
00017     Setting_ABstate_Table();
00018 
00019     chA.rise(this,&QEI::PulseCount);
00020     chA.fall(this,&QEI::PulseCount);
00021     chB.rise(this,&QEI::PulseCount);
00022     chB.fall(this,&QEI::PulseCount);
00023 
00024     pre_ABstate = (chA.read() << 1)|(chB.read());
00025     _timer -> start();
00026 }
00027 
00028 void QEI::Setting_ABstate_Table()
00029 {
00030     for(int i =0; i<15; i++) {
00031         if     (i == 0b1101 || i == 0b0100 || i == 0b0010 || i == 0b1011) ABstate_Table[i] =  1;
00032         else if(i == 0b1110 || i == 0b0111 || i == 0b0001 || i == 0b1000) ABstate_Table[i] = -1;
00033         else                                                              ABstate_Table[i] =  0;
00034     }
00035 }
00036 
00037 void QEI::useAvePRS()
00038 {
00039     _ticker -> attach_us(this,&QEI::MoveAve_pulse_to_RPS,3000); //3000usごと
00040 }
00041 
00042 
00043 void QEI::PulseCount()
00044 {
00045     uint8_t ABstate = (chA.read() << 1)|(chB.read());
00046     uint8_t ENCstate = (pre_ABstate << 2)|ABstate;
00047     pre_ABstate = ABstate;
00048 
00049     if(ENCstate == 0b1101 || ENCstate == 0b0100 || ENCstate == 0b0010 || ENCstate == 0b1011) pulse++;
00050     else if(ENCstate == 0b1110 || ENCstate == 0b0111 || ENCstate == 0b0001 || ENCstate == 0b1000) pulse--;
00051 }
00052 
00053 
00054 void QEI::MoveAve_pulse()
00055 {
00056     for(int i =0; i<data_sizeof-1; i++) pulseData[(data_sizeof-1)-i] = pulseData[(data_sizeof-2)-i];
00057     pulseData[0] = pulse;
00058     pulseAve = 0;
00059     for(int i =0; i<data_sizeof; i++) pulseAve += pulseData[i];
00060     pulseAve /= data_sizeof;
00061     pulse = 0;
00062 }
00063 
00064 
00065 void QEI::MoveAve_pulse_to_RPS()
00066 {
00067     //時間計測
00068     //static double pre_time=0;  //修正1:これじゃRPS複数とるとやばいことになる
00069     double current_time = _timer -> read();
00070     double dt = current_time - pre_time;
00071     //移動平均算出
00072     for(int i =0; i<data_sizeof-1; i++) pulseData[(data_sizeof-1)-i] = pulseData[(data_sizeof-2)-i];
00073     pulseData[0] = pulse;
00074     pulseAve = 0;
00075     for(int i =0; i<data_sizeof; i++) pulseAve += pulseData[i];
00076     pulseAve /= data_sizeof;
00077     pulse = 0;
00078 
00079     //RPS算出
00080     rps = pulseAve/(_PPR*4 * dt);
00081 
00082     pre_time = current_time;
00083 
00084 }
00085 
00086 double QEI::getRPS()
00087 {
00088     return rps*NrotDIR;
00089 }
00090 
00091 double QEI::Odometry(double WheelDia)
00092 {
00093     return (WheelDia*PI*pulse/_PPR)*NrotDIR;
00094 }
00095 
00096 void QEI::rotReverce()
00097 {
00098     NrotDIR = -1;
00099 }
00100 
00101 //パルスの確認
00102 int QEI::returnPulse()
00103 {
00104     return pulse;
00105 }
00106 
00107 //********************処理をもっと頭良さげにしたものが2********************
00108 //*********************違いはコメントアウトで説明*************************
00109 void QEI::useAvePRS2()
00110 {
00111     _ticker -> attach_us(this,&QEI::MoveAve_pulse_to_RPS2,3333); //3333usごと
00112 }
00113 
00114 void QEI::PulseCount2()
00115 {
00116     uint8_t ABstate = (chA.read() << 1)|(chB.read());
00117     uint8_t ENCstate = (pre_ABstate << 2)|ABstate;
00118     pre_ABstate = ABstate;
00119     pulse += ABstate_Table[ENCstate];  //エンコーダの表を参照するようになった.表の反転でパルスの増減方向が変えられる
00120 }
00121 
00122 void QEI::rotReverce2()//表自体を反転させることで正転方向を変更する
00123 {
00124     for(int i =0; i<15; i++) ABstate_Table[i] *= -1;
00125 }
00126 
00127 void QEI::MoveAve_pulse_to_RPS2()
00128 {
00129     static int WriteNum_of_pulseData = 0;  //古い情報をズラして格納していたが無駄なので消去.代わりに配列のWriteNum_of_pulseDataのところに書き込んでいく
00130     //時間計測
00131     double current_time = _timer -> read();
00132     double dt = current_time - pre_time;
00133     //移動平均算出
00134     pulseData[WriteNum_of_pulseData] = pulse;
00135     WriteNum_of_pulseData == data_sizeof-1 ? WriteNum_of_pulseData = 0 : WriteNum_of_pulseData++;//WriteNum_of_pulseDataが配列以上になったら0に戻す
00136     pulseAve = 0;
00137     for(int i =0; i<data_sizeof; i++) pulseAve += pulseData[i];
00138     pulseAve /= data_sizeof;
00139     pulse = 0;
00140 
00141     //RPS算出
00142     rps = pulseAve/(_PPR*4 * dt);
00143 
00144     pre_time = current_time;
00145 }
00146 
00147 //MoveAve_pulse_to_RPS2で出したRPS値を返す
00148 double QEI::getRPS2()
00149 {
00150     return rps;
00151 }
00152 
00153 //オドメトリ用関数2
00154 double QEI::Odometry2(double WheelDia)
00155 {
00156     return (WheelDia*PI*pulse/_PPR);
00157 }