自分用ロータリーエンコーダのライブラリ. Z相のパルスは一切使用していない. タイマ割り込みを使ってRPSをとる場合,MvgAve的なものが含まれる関数によって可能だが,正直いらない気がする.
QEI.cpp
- Committer:
- ttrist
- Date:
- 2020-10-29
- Revision:
- 0:13bff3e4fe87
- Child:
- 1:4aca4f190ab1
File content as of revision 0:13bff3e4fe87:
#include "QEI.h" #define PI 3.141592653589793 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~インスタンス生成~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // 移動平均していないRPSをとる場合はこれを使う. // 引数は(A相のピン,B相のピン,1回転のパルスppr) QEI::QEI(PinName A, PinName B, int ppr, Timer *timer): ch_a(A), ch_b(B) { _timer = timer; _ppr = ppr; pre_time=0; init(); } // 移動平均版RPSを利用する場合はこれを使う. // 引数は(A相のピン,B相のピン,1回転のパルスppr,timerインスタンス,tickerインスタンス) QEI::QEI(PinName A, PinName B, int ppr, Timer *timer, Ticker *ticker): ch_a(A), ch_b(B) { _timer = timer; _ticker = ticker; _ppr = ppr; pre_time=0; init(); } //~~~~~~~~~~~以下の2つは上記のTimer,Tickerをクラス内で作ったバージョン~~~~~~~~~~~~~~ //~~~~~~~~~~~引数にTimer,Tickerがいらなくなるため,コードがスッキリする~~~~~~~~~~~~~~~~ //~~~~~~~~~~~ただし,複数のエンコーダを同時に扱うときに正しく動作するかは不明~~~~~~~~~~~~~ //~~~~~~~~~~~正常に動いたなら上の2つはいらなくなる~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // 引数は(A相のピン,B相のピン,1回転のパルスppr) QEI::QEI(PinName A, PinName B, int ppr): ch_a(A), ch_b(B) { Timer timer; Ticker ticker; _timer = &timer; _ticker = &ticker; _ppr = ppr; pre_time=0; init(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~以下関数など~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // 初期設定 void QEI::init() { // 割り込みピン設定 ch_a.rise(this, &QEI::countPulse); ch_a.fall(this, &QEI::countPulse); ch_b.rise(this, &QEI::countPulse); ch_b.fall(this, &QEI::countPulse); resetVariable(); // 変数クリア makeEncoderTable(); _timer -> start(); } // 変数リセット void QEI::resetVariable() { for(int i =0; i<data_sizeof; i++) pulse_data[i] = 0; pre_state_ab = (ch_a.read() << 1) | ch_b.read(); mvg_ave_rps = 0; pulse = 0; } // エンコーダの状態を2進数4桁で表した際のパルス数変化の表↓を作るだけ // encoder_table[15] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1} って定義できなかった. void QEI::makeEncoderTable() { for(int i =0; i<15; i++) { if (i == 0b1101 || i == 0b1011 || i == 0b0100 || i == 0b0010) encoder_table[i] = 1; else if(i == 0b1110 || i == 0b1000 || i == 0b0111 || i == 0b0001) encoder_table[i] = -1; else encoder_table[i] = 0; } } // エンコーダにとっての正転方向を反対にする // 表の正負を反転にしてるだけ void QEI::reverseEncoderDirection() { for(int i =0; i<15; i++) encoder_table[i] *= -1; } // パルスをカウントする関数 void QEI::countPulse() { uint8_t state_ab = (ch_a.read() << 1) | ch_b.read(); uint8_t state_encoder = (pre_state_ab << 2) | state_ab; pulse += encoder_table[state_encoder]; //エンコーダ表を参照. pre_state_ab = state_ab; //旧データ保存 } // 回転角度を計算する関数 double QEI::getDeg() { return 360 * double(pulse) / (_ppr * 4); } // パルスの確認 int QEI::getPulse() { return pulse; } // 移動平均していないRPSを計算する関数 double QEI::getRPS() { double current_time = _timer -> read(); //RPS算出 double rps = ( double(pulse - pre_pulse)/(_ppr * 4) ) // 1回転分のパルス数はppr*4 / //--------------------------------------------- ← 割り算の横棒 ( current_time - pre_time ) // 俗に言うdt ; pre_time = current_time; pre_pulse = pulse; return rps; } // 移動平均(MvgAve)したRPSを返す double QEI::getMvgAveRPS() { return mvg_ave_rps; } // 移動平均(MvgAve)したRPSを使う場合に記述. // 引数us毎に移動平均をとるために割り込みが発生する. // 引数は小さい方がいいが,3500あたりがおすすめ // なお引数を指定しない場合は3333us毎に割りこむ. // (注)getMvgAveRPS()以外の機能は使えなくなる. void QEI::useMvgAvePRS(int interrupt_time_us) { _ticker -> attach_us(this, &QEI::culculateMvgAveRPS, interrupt_time_us); editing_num = 0; } void QEI::useMvgAvePRS() { _ticker -> attach_us(this, &QEI::culculateMvgAveRPS, 3333); editing_num = 0; } //移動平均(MvgAve)したRPSを計算する関数 void QEI::culculateMvgAveRPS() { //時間計測 double current_time = _timer -> read(); //移動平均算出 pulse_data[editing_num] = pulse; editing_num == data_sizeof-1 ? editing_num = 0 : editing_num++;//editing_numが配列以上になったら0に戻す pulse_average = 0; for(int i =0; i < data_sizeof; i++) pulse_average += pulse_data[i]; pulse_average /= data_sizeof; pulse = 0; // オーバーフロー対策.この処理をするため,他の機能が使えなくなる. //RPS算出 mvg_ave_rps = ( pulse_average/(_ppr * 4) ) // 1回転分のパルス数はppr*4 / //--------------------------------------------- ← 割り算の横棒 ( current_time - pre_time ) // 俗に言うdt ; pre_time = current_time; }