ヒロキ モリ / Encoder

QEILibrary

Dependents:   MotorIdentification MotorPIcontrol Motor_P_PositionControl

Encoder.cpp@0:6d62a5947f59, 2019-03-18 (annotated)

Committer:
porizou3
Date:
Mon Mar 18 04:46:34 2019 +0000
Revision:
0:6d62a5947f59
commit;

Who changed what in which revision?

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