自分用QEI
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 20 2022 00:45:32 by
1.7.2