Revision 0:28acb74250ee, committed 2018-08-08

Comitter:

porizou

Date:

Wed Aug 08 14:04:47 2018 +0000

Commit message:

first commit;

Changed in this revision

diff -r 000000000000 -r 28acb74250ee Encoder.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp	Wed Aug 08 14:04:47 2018 +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_;
+}

diff -r 000000000000 -r 28acb74250ee Encoder.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.h	Wed Aug 08 14:04:47 2018 +0000
@@ -0,0 +1,102 @@
+/* --- エンコーダから回転角を取得して速度を計算するクラス --- */
+
+#ifndef __Encoder_h
+#define __Encoder_h
+
+/**
+ * Includes.
+ */
+#include "mbed.h"
+#include "QEI.h"
+
+/**
+ * Defines.
+ */
+#define PI 3.14159265358979
+
+#define LPF 0.2 //0~1 速度計算,LPF係数
+
+class Encoder {
+
+public:
+
+    /**
+     *
+     * A エンコーダA相ピン変化割り込みピン
+     * B エンコーダB相ピン変化割り込みピン
+     * pulseNum 一回転辺りのパルス数
+     * aSample 回転角取得周期[s]
+     * vSample 回転速度取得周期[s]
+    */      
+    Encoder(PinName A , PinName B , int pulseNum , float aSample, float vSample);
+
+    /* 初期値の設定 */
+    void initParam(void);
+
+    /* 回転角を取得するタイマー割り込み関数　*/
+    void calcAngle(void);
+    
+    /* 回転速度を取得するタイマー割り込み関数　*/
+    void calcVelocity(void);
+
+    /* 回転速度を計算する関数 */
+    float getVelocity(void); //回転速度[rad/s]の取得
+
+    int getPulses(void); //パルス数の取得
+
+    float getAngle(void); //回転角度[rad]の取得
+
+    float getRevolution(void); //回転数の取得
+
+private:
+
+    QEI qei;
+
+    Ticker AngleTicker; // 回転角度計算タイマー割り込み
+    Ticker VelocityTicker; //回転速度タイマー割り込み
+
+    int pulseNum_;
+    
+    float aSample_;
+    float vSample_;
+
+    float  Angle;   // 回転角度 [rad]
+    float  pAngle;  // 前回の回転角度 [rad]
+
+    float  Velocity;  // 回転速度 [rad/s]
+    float  pVelocity; // 前回の回転速度 [rad/s]
+
+
+};
+
+#endif 
+
+
+
+
+
+
+
+                        
+
+                            
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+