Revision 0:6d62a5947f59, committed 2019-03-18

Comitter:

porizou3

Date:

Mon Mar 18 04:46:34 2019 +0000

Commit message:

commit;

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.h	Mon Mar 18 04:46:34 2019 +0000
@@ -0,0 +1,72 @@
+/* --- エンコーダから回転角を取得して速度を計算するクラス --- */
+ 
+#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 
\ No newline at end of file