Diff: Encoder.cpp

Revision:

0:6d62a5947f59

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp	Mon Mar 18 04:46:34 2019 +0000
@@ -0,0 +1,64 @@
+#include "Encoder.h"
+ 
+Encoder::Encoder(PinName A , PinName B ,  int pulseNum ,  float aSample, float vSample) : 
+                             qei(A , B , NC , pulseNum , QEI::X4_ENCODING) {
+ 
+    pulseNum_ = pulseNum;
+    aSample_  = aSample;
+    vSample_  = vSample;
+ 
+    initParam();
+ 
+    /* 回転角度計算タイマー割り込み開始 */
+    AngleTicker.attach(this , &Encoder::calcAngle , aSample_);
+    /* 回転速度計算タイマー割り込み開始 */
+    VelocityTicker.attach(this , &Encoder::calcVelocity , vSample_);
+}
+ 
+void Encoder::initParam(void) {
+ 
+    qei.reset();
+    /* 現在の回転角を原点に設定 */
+    Angle = pAngle = (float)qei.getPulses()/(float)pulseNum_*2.0*PI;
+ 
+    Velocity  = 0.0;
+    pVelocity = 0.0;
+}
+ 
+void Encoder::calcAngle(void) {
+    /* エンコーダから回転角を取得 */
+    Angle = (float)qei.getPulses()/(float)pulseNum_*2.0*PI;
+}
+ 
+void Encoder::calcVelocity(void) {
+    float dAngle;
+ 
+    dAngle = Angle - pAngle ; // 角度の差分を計算
+    pAngle = Angle ; // 前回の回転角を更新
+ 
+    //while(dAngle > PI){ dAngle -= 2*PI;} //角度の差分の範囲を-π~πにする
+    //while(dAngle <-PI){ dAngle += 2*PI;}
+ 
+    /* 速度を計算(回転角の微分 dAngle/Sample_)して、一次LPFに通す
+       現在の速度 = LPF係数 × 前回の速度 + 現在の速度 × (1 - LPF係数) */
+    Velocity = LPF * pVelocity + (dAngle/vSample_) * (1.0-LPF);
+    pVelocity = Velocity; //前回の速度を更新
+}
+ 
+float Encoder::getVelocity(void) {
+    return Velocity;
+}
+ 
+int Encoder::getPulses(void) {
+    return qei.getPulses();
+}
+ 
+float Encoder::getAngle(void) {
+ 
+    return Angle;
+}
+ 
+float Encoder::getRevolution(void) {
+ 
+    return qei.getPulses() / pulseNum_;
+}
\ No newline at end of file