yosuke kitajima / QEI2_verKi

自分用ロータリーエンコーダのライブラリ. Z相のパルスは一切使用していない． タイマ割り込みを使ってRPSをとる場合，MvgAve的なものが含まれる関数によって可能だが，正直いらない気がする．

QEI.cpp@8:6597cf4c8ce4, 2020-11-19 (annotated)

Committer:
ttrist
Date:
Thu Nov 19 16:02:19 2020 +0000
Revision:
8:6597cf4c8ce4
Parent:
6:deb3445444c5
Child:
9:9bb65948cb62
not change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ttrist 0:13bff3e4fe87 1 #include "QEI.h"
ttrist 0:13bff3e4fe87 2 #define PI 3.141592653589793
ttrist 0:13bff3e4fe87 3
ttrist 0:13bff3e4fe87 4 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 5 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 6 //~~~~~~~~~~~~~~~~~~インスタンス生成~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 7 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 8 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 9
ttrist 0:13bff3e4fe87 10 // 移動平均していないRPSをとる場合はこれを使う．
ttrist 0:13bff3e4fe87 11 // 引数は(A相のピン,B相のピン,1回転のパルスppr)
ttrist 0:13bff3e4fe87 12 QEI::QEI(PinName A, PinName B, int ppr, Timer *timer): ch_a(A), ch_b(B)
ttrist 0:13bff3e4fe87 13 {
ttrist 0:13bff3e4fe87 14 _timer = timer;
ttrist 0:13bff3e4fe87 15 _ppr = ppr;
ttrist 0:13bff3e4fe87 16 pre_time=0;
ttrist 0:13bff3e4fe87 17 init();
ttrist 0:13bff3e4fe87 18 }
ttrist 0:13bff3e4fe87 19
ttrist 0:13bff3e4fe87 20 // 移動平均版RPSを利用する場合はこれを使う．
ttrist 0:13bff3e4fe87 21 // 引数は(A相のピン,B相のピン,1回転のパルスppr,timerインスタンス,tickerインスタンス)
ttrist 0:13bff3e4fe87 22 QEI::QEI(PinName A, PinName B, int ppr, Timer *timer, Ticker *ticker): ch_a(A), ch_b(B)
ttrist 0:13bff3e4fe87 23 {
ttrist 0:13bff3e4fe87 24 _timer = timer;
ttrist 0:13bff3e4fe87 25 _ticker = ticker;
ttrist 0:13bff3e4fe87 26 _ppr = ppr;
ttrist 0:13bff3e4fe87 27 pre_time=0;
ttrist 0:13bff3e4fe87 28 init();
ttrist 0:13bff3e4fe87 29 }
ttrist 0:13bff3e4fe87 30
ttrist 0:13bff3e4fe87 31 //~~~~~~~~~~~以下の2つは上記のTimer,Tickerをクラス内で作ったバージョン~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 32 //~~~~~~~~~~~引数にTimer,Tickerがいらなくなるため，コードがスッキリする~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 33 //~~~~~~~~~~~ただし，複数のエンコーダを同時に扱うときに正しく動作するかは不明~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 34 //~~~~~~~~~~~正常に動いたなら上の2つはいらなくなる~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 35
ttrist 0:13bff3e4fe87 36
ttrist 0:13bff3e4fe87 37 // 引数は(A相のピン,B相のピン,1回転のパルスppr)
ttrist 0:13bff3e4fe87 38 QEI::QEI(PinName A, PinName B, int ppr): ch_a(A), ch_b(B)
ttrist 0:13bff3e4fe87 39 {
ttrist 0:13bff3e4fe87 40 Timer timer;
ttrist 0:13bff3e4fe87 41 Ticker ticker;
ttrist 0:13bff3e4fe87 42 _timer = &timer;
ttrist 0:13bff3e4fe87 43 _ticker = &ticker;
ttrist 0:13bff3e4fe87 44 _ppr = ppr;
ttrist 0:13bff3e4fe87 45 pre_time=0;
ttrist 0:13bff3e4fe87 46 init();
ttrist 0:13bff3e4fe87 47 }
ttrist 0:13bff3e4fe87 48
ttrist 0:13bff3e4fe87 49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 50 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 51 //~~~~~~~~~~~~~~~~~~以下関数など~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 52 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 53 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ttrist 0:13bff3e4fe87 54
ttrist 0:13bff3e4fe87 55 // 初期設定
ttrist 0:13bff3e4fe87 56 void QEI::init()
ttrist 0:13bff3e4fe87 57 {
ttrist 0:13bff3e4fe87 58 // 割り込みピン設定
ttrist 0:13bff3e4fe87 59 ch_a.rise(this, &QEI::countPulse);
ttrist 0:13bff3e4fe87 60 ch_a.fall(this, &QEI::countPulse);
ttrist 0:13bff3e4fe87 61 ch_b.rise(this, &QEI::countPulse);
ttrist 0:13bff3e4fe87 62 ch_b.fall(this, &QEI::countPulse);
ttrist 0:13bff3e4fe87 63
ttrist 0:13bff3e4fe87 64 resetVariable(); // 変数クリア
ttrist 0:13bff3e4fe87 65
ttrist 0:13bff3e4fe87 66 makeEncoderTable();
ttrist 0:13bff3e4fe87 67 _timer -> start();
ttrist 0:13bff3e4fe87 68 }
ttrist 0:13bff3e4fe87 69
ttrist 0:13bff3e4fe87 70 // 変数リセット
ttrist 0:13bff3e4fe87 71 void QEI::resetVariable()
ttrist 0:13bff3e4fe87 72 {
ttrist 4:1789feb609ac 73 for(int i =0; i<_MVGAVE_ARRAY_SIZE; i++) pulse_data[i] = 0;
ttrist 0:13bff3e4fe87 74 pre_state_ab = (ch_a.read() << 1) | ch_b.read();
ttrist 0:13bff3e4fe87 75 mvg_ave_rps = 0;
ttrist 0:13bff3e4fe87 76 pulse = 0;
ttrist 0:13bff3e4fe87 77 }
ttrist 0:13bff3e4fe87 78
ttrist 0:13bff3e4fe87 79 // エンコーダの状態を2進数4桁で表した際のパルス数変化の表↓を作るだけ
ttrist 0:13bff3e4fe87 80 // encoder_table[15] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1} って定義できなかった．
ttrist 0:13bff3e4fe87 81 void QEI::makeEncoderTable()
ttrist 0:13bff3e4fe87 82 {
ttrist 0:13bff3e4fe87 83 for(int i =0; i<15; i++) {
ttrist 0:13bff3e4fe87 84 if (i == 0b1101 || i == 0b1011 || i == 0b0100 || i == 0b0010) encoder_table[i] = 1;
ttrist 0:13bff3e4fe87 85 else if(i == 0b1110 || i == 0b1000 || i == 0b0111 || i == 0b0001) encoder_table[i] = -1;
ttrist 0:13bff3e4fe87 86 else encoder_table[i] = 0;
ttrist 0:13bff3e4fe87 87 }
ttrist 0:13bff3e4fe87 88 }
ttrist 0:13bff3e4fe87 89
ttrist 0:13bff3e4fe87 90
ttrist 0:13bff3e4fe87 91 // エンコーダにとっての正転方向を反対にする
ttrist 0:13bff3e4fe87 92 // 表の正負を反転にしてるだけ
ttrist 0:13bff3e4fe87 93 void QEI::reverseEncoderDirection()
ttrist 0:13bff3e4fe87 94 {
ttrist 0:13bff3e4fe87 95 for(int i =0; i<15; i++) encoder_table[i] *= -1;
ttrist 0:13bff3e4fe87 96 }
ttrist 0:13bff3e4fe87 97
ttrist 0:13bff3e4fe87 98
ttrist 0:13bff3e4fe87 99
ttrist 0:13bff3e4fe87 100 // パルスをカウントする関数
ttrist 0:13bff3e4fe87 101 void QEI::countPulse()
ttrist 0:13bff3e4fe87 102 {
ttrist 0:13bff3e4fe87 103 uint8_t state_ab = (ch_a.read() << 1) | ch_b.read();
ttrist 0:13bff3e4fe87 104 uint8_t state_encoder = (pre_state_ab << 2) | state_ab;
ttrist 0:13bff3e4fe87 105 pulse += encoder_table[state_encoder]; //エンコーダ表を参照．
ttrist 0:13bff3e4fe87 106 pre_state_ab = state_ab; //旧データ保存
ttrist 0:13bff3e4fe87 107 }
ttrist 0:13bff3e4fe87 108
ttrist 0:13bff3e4fe87 109
ttrist 8:6597cf4c8ce4 110 // パルスの確認
ttrist 8:6597cf4c8ce4 111 int QEI::getPulse()
ttrist 8:6597cf4c8ce4 112 {
ttrist 8:6597cf4c8ce4 113 return pulse;
ttrist 8:6597cf4c8ce4 114 }
ttrist 8:6597cf4c8ce4 115
ttrist 8:6597cf4c8ce4 116
ttrist 0:13bff3e4fe87 117 // 回転角度を計算する関数
ttrist 1:4aca4f190ab1 118 float QEI::getDeg()
ttrist 0:13bff3e4fe87 119 {
ttrist 1:4aca4f190ab1 120 return 360 * float(pulse) / (_ppr * 4);
ttrist 0:13bff3e4fe87 121 }
ttrist 0:13bff3e4fe87 122
ttrist 0:13bff3e4fe87 123
ttrist 0:13bff3e4fe87 124 // 移動平均していないRPSを計算する関数
ttrist 1:4aca4f190ab1 125 float QEI::getRPS()
ttrist 0:13bff3e4fe87 126 {
ttrist 6:deb3445444c5 127 uint32_t current_time = _timer -> read();
ttrist 0:13bff3e4fe87 128
ttrist 0:13bff3e4fe87 129 //RPS算出
ttrist 1:4aca4f190ab1 130 float rps =
ttrist 1:4aca4f190ab1 131 ( float(pulse - pre_pulse)/(_ppr * 4) ) // 1回転分のパルス数はppr*4
ttrist 0:13bff3e4fe87 132 / //--------------------------------------------- ← 割り算の横棒
ttrist 0:13bff3e4fe87 133 ( current_time - pre_time ) // 俗に言うdt
ttrist 0:13bff3e4fe87 134 ;
ttrist 0:13bff3e4fe87 135
ttrist 0:13bff3e4fe87 136 pre_time = current_time;
ttrist 0:13bff3e4fe87 137 pre_pulse = pulse;
ttrist 0:13bff3e4fe87 138 return rps;
ttrist 0:13bff3e4fe87 139 }
ttrist 0:13bff3e4fe87 140
ttrist 0:13bff3e4fe87 141
ttrist 0:13bff3e4fe87 142 // 移動平均(MvgAve)したRPSを返す
ttrist 1:4aca4f190ab1 143 float QEI::getMvgAveRPS()
ttrist 0:13bff3e4fe87 144 {
ttrist 0:13bff3e4fe87 145 return mvg_ave_rps;
ttrist 0:13bff3e4fe87 146 }
ttrist 0:13bff3e4fe87 147
ttrist 0:13bff3e4fe87 148 // 移動平均(MvgAve)したRPSを使う場合に記述．
ttrist 0:13bff3e4fe87 149 // 引数us毎に移動平均をとるために割り込みが発生する．
ttrist 0:13bff3e4fe87 150 // 引数は小さい方がいいが，3500あたりがおすすめ
ttrist 0:13bff3e4fe87 151 // なお引数を指定しない場合は3333us毎に割りこむ．
ttrist 0:13bff3e4fe87 152 // (注)getMvgAveRPS（）以外の機能は使えなくなる．
ttrist 0:13bff3e4fe87 153 void QEI::useMvgAvePRS(int interrupt_time_us)
ttrist 0:13bff3e4fe87 154 {
ttrist 0:13bff3e4fe87 155 _ticker -> attach_us(this, &QEI::culculateMvgAveRPS, interrupt_time_us);
ttrist 0:13bff3e4fe87 156 editing_num = 0;
ttrist 0:13bff3e4fe87 157 }
ttrist 0:13bff3e4fe87 158 void QEI::useMvgAvePRS()
ttrist 0:13bff3e4fe87 159 {
ttrist 0:13bff3e4fe87 160 _ticker -> attach_us(this, &QEI::culculateMvgAveRPS, 3333);
ttrist 0:13bff3e4fe87 161 editing_num = 0;
ttrist 0:13bff3e4fe87 162 }
ttrist 0:13bff3e4fe87 163
ttrist 0:13bff3e4fe87 164 //移動平均(MvgAve)したRPSを計算する関数
ttrist 0:13bff3e4fe87 165 void QEI::culculateMvgAveRPS()
ttrist 0:13bff3e4fe87 166 {
ttrist 0:13bff3e4fe87 167 //時間計測
ttrist 6:deb3445444c5 168 uint32_t current_time = _timer -> read();
ttrist 0:13bff3e4fe87 169
ttrist 0:13bff3e4fe87 170 //移動平均算出
ttrist 0:13bff3e4fe87 171 pulse_data[editing_num] = pulse;
ttrist 4:1789feb609ac 172 editing_num == _MVGAVE_ARRAY_SIZE-1 ? editing_num = 0 : editing_num++;//editing_numが配列以上になったら0に戻す
ttrist 0:13bff3e4fe87 173 pulse_average = 0;
ttrist 4:1789feb609ac 174 for(int i =0; i < _MVGAVE_ARRAY_SIZE; i++) pulse_average += pulse_data[i];
ttrist 4:1789feb609ac 175 pulse_average /= _MVGAVE_ARRAY_SIZE;
ttrist 0:13bff3e4fe87 176 pulse = 0; // オーバーフロー対策．この処理をするため，他の機能が使えなくなる．
ttrist 0:13bff3e4fe87 177
ttrist 0:13bff3e4fe87 178 //RPS算出
ttrist 0:13bff3e4fe87 179 mvg_ave_rps =
ttrist 0:13bff3e4fe87 180 ( pulse_average/(_ppr * 4) ) // 1回転分のパルス数はppr*4
ttrist 0:13bff3e4fe87 181 / //--------------------------------------------- ← 割り算の横棒
ttrist 0:13bff3e4fe87 182 ( current_time - pre_time ) // 俗に言うdt
ttrist 0:13bff3e4fe87 183 ;
ttrist 0:13bff3e4fe87 184
ttrist 0:13bff3e4fe87 185 pre_time = current_time;
ttrist 0:13bff3e4fe87 186 }