Hiroki Mori / Encoder

Encoder QEi

Encoder.cpp@0:28acb74250ee, 2018-08-08 (annotated)

Committer:
porizou
Date:
Wed Aug 08 14:04:47 2018 +0000
Revision:
0:28acb74250ee
first commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
porizou 0:28acb74250ee 1 #include "Encoder.h"
porizou 0:28acb74250ee 2
porizou 0:28acb74250ee 3 Encoder::Encoder(PinName A , PinName B , int pulseNum , float aSample, float vSample) :
porizou 0:28acb74250ee 4 qei(A , B , NC , pulseNum , QEI::X4_ENCODING) {
porizou 0:28acb74250ee 5
porizou 0:28acb74250ee 6 pulseNum_ = pulseNum;
porizou 0:28acb74250ee 7 aSample_ = aSample;
porizou 0:28acb74250ee 8 vSample_ = vSample;
porizou 0:28acb74250ee 9
porizou 0:28acb74250ee 10 initParam();
porizou 0:28acb74250ee 11
porizou 0:28acb74250ee 12 /* 回転角度計算タイマー割り込み開始 */
porizou 0:28acb74250ee 13 AngleTicker.attach(this , &Encoder::calcAngle , aSample_);
porizou 0:28acb74250ee 14 /* 回転速度計算タイマー割り込み開始 */
porizou 0:28acb74250ee 15 VelocityTicker.attach(this , &Encoder::calcVelocity , vSample_);
porizou 0:28acb74250ee 16 }
porizou 0:28acb74250ee 17
porizou 0:28acb74250ee 18 void Encoder::initParam(void) {
porizou 0:28acb74250ee 19
porizou 0:28acb74250ee 20 qei.reset();
porizou 0:28acb74250ee 21 /* 現在の回転角を原点に設定 */
porizou 0:28acb74250ee 22 Angle = pAngle = (float)qei.getPulses()/(float)pulseNum_*2.0*PI;
porizou 0:28acb74250ee 23
porizou 0:28acb74250ee 24 Velocity = 0.0;
porizou 0:28acb74250ee 25 pVelocity = 0.0;
porizou 0:28acb74250ee 26 }
porizou 0:28acb74250ee 27
porizou 0:28acb74250ee 28 void Encoder::calcAngle(void) {
porizou 0:28acb74250ee 29 /* エンコーダから回転角を取得 */
porizou 0:28acb74250ee 30 Angle = (float)qei.getPulses()/(float)pulseNum_*2.0*PI;
porizou 0:28acb74250ee 31 }
porizou 0:28acb74250ee 32
porizou 0:28acb74250ee 33 void Encoder::calcVelocity(void) {
porizou 0:28acb74250ee 34 float dAngle;
porizou 0:28acb74250ee 35
porizou 0:28acb74250ee 36 dAngle = Angle - pAngle ; // 角度の差分を計算
porizou 0:28acb74250ee 37 pAngle = Angle ; // 前回の回転角を更新
porizou 0:28acb74250ee 38
porizou 0:28acb74250ee 39 //while(dAngle > PI){ dAngle -= 2*PI;} //角度の差分の範囲を-π~πにする
porizou 0:28acb74250ee 40 //while(dAngle <-PI){ dAngle += 2*PI;}
porizou 0:28acb74250ee 41
porizou 0:28acb74250ee 42 /* 速度を計算(回転角の微分 dAngle/Sample_)して、一次LPFに通す
porizou 0:28acb74250ee 43 現在の速度 = LPF係数 × 前回の速度 + 現在の速度 × (1 - LPF係数) */
porizou 0:28acb74250ee 44 Velocity = LPF * pVelocity + (dAngle/vSample_) * (1.0-LPF);
porizou 0:28acb74250ee 45 pVelocity = Velocity; //前回の速度を更新
porizou 0:28acb74250ee 46 }
porizou 0:28acb74250ee 47
porizou 0:28acb74250ee 48 float Encoder::getVelocity(void) {
porizou 0:28acb74250ee 49 return Velocity;
porizou 0:28acb74250ee 50 }
porizou 0:28acb74250ee 51
porizou 0:28acb74250ee 52 int Encoder::getPulses(void) {
porizou 0:28acb74250ee 53 return qei.getPulses();
porizou 0:28acb74250ee 54 }
porizou 0:28acb74250ee 55
porizou 0:28acb74250ee 56 float Encoder::getAngle(void) {
porizou 0:28acb74250ee 57
porizou 0:28acb74250ee 58 return Angle;
porizou 0:28acb74250ee 59 }
porizou 0:28acb74250ee 60
porizou 0:28acb74250ee 61 float Encoder::getRevolution(void) {
porizou 0:28acb74250ee 62
porizou 0:28acb74250ee 63 return qei.getPulses() / pulseNum_;
porizou 0:28acb74250ee 64 }