Diff: EC.cpp

Revision:

0:3d9101dd0061

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EC.cpp	Wed Oct 31 09:56:08 2018 +0000
@@ -0,0 +1,190 @@
+#include "mbed.h"
+#include "EC.h"
+
+int Ec::defsolution;
+double Ec::deftime;
+//ピン変化割り込み関数の定義
+void Ec::upA()
+{
+    stateA=1;
+    if(stateB==0&&S==0) {
+        S=1;
+    } else if(stateB==1&&S==3) {
+        S=2;
+    }
+}
+void Ec::downA()
+{
+    stateA=0;
+    if(stateB==1&&S==2) {
+        S=3;
+    } else if(stateB==0&&S==1) {
+        S=0;
+        count--;
+    }
+}
+void Ec::upB()
+{
+    stateB=1;
+    if(stateA==1&&S==1) {
+        S=2;
+    } else if(stateA==0&&S==0) {
+        S=3;
+    }
+}
+void Ec::downB()
+{
+    stateB=0;
+    if(stateA==0&&S==3) {
+        count++;
+        S=0;
+    } else if(stateA==1&&S==2) {
+        S=1;
+    }
+}
+
+void Ec::upZ()
+{
+    if(first==false) {
+        static int res_count;
+        rev++;
+        first=true;
+        now_time=timer.read();
+        RPM=60/(now_time-old_time);
+        if((RPM_old-RPM)>RPM_th) {
+            if(res_count >= 5) {
+                printf("\r\n CAUTION : speed downed drastically\r\n");
+                NVIC_SystemReset();
+                res_count=0;
+            } else {
+                res_count++;
+            }
+        } else {
+            res_count=0;
+        }
+        RPM_old=RPM;
+        old_time=timer.read();
+    }
+}
+
+void Ec::downZ()
+{
+    first=false;
+}
+
+
+//コンストラクタの定義
+//main関数の前に必ず一度宣言する
+//第一・第二引数はエンコーダのA・B相のピン名
+//第三院数はエンコーダの分解能
+
+Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
+{
+
+    if((signalA!=NC)&&(signalB!=NC)) {
+        S=0;
+        stateA=0;
+        stateB=0;
+        count=0;
+        pre_count=0.0;
+        timer.start();
+        signalA_.rise(callback(this,&Ec::upA));
+        signalA_.fall(callback(this,&Ec::downA));
+        signalB_.rise(callback(this,&Ec::upB));
+        signalB_.fall(callback(this,&Ec::downB));
+    }
+    if(signalZ!=NC) {
+        first=false;
+        rev=0;
+        old_time=0;
+        RPM=0;
+        RPM_old=0;
+        signalZ_.rise(callback(this,&Ec::upZ));
+        signalZ_.fall(callback(this,&Ec::downZ));
+    }
+    dt=t;
+    solution=s;
+    defsolution=s;
+    RPM_th=250;
+    count_to_distance_mm_ = 0;
+    gear_rate_ = 1;
+    is_diameter_set = 0;
+}
+
+int Ec::getCount()
+{
+    return count;
+}
+
+void Ec::CalOmega()
+{
+    omega=(count-pre_count)*2*M_pi/(solution*dt);
+    pre_count=count;
+}
+
+double Ec::getOmega()
+{
+    return omega;
+}
+
+double Ec::getPreCount()
+{
+    precount=count+S/4.0;
+    return precount;
+}
+/*reset関数の定義*/
+/*エンコーダを初期状態に戻すことができる*/
+void Ec::reset()
+{
+    S=0;
+    stateA=0;
+    stateB=0;
+    count=0;
+    pre_count=0.0,omega=0;
+    rev=0;
+    now_time=0;
+    old_time=0;
+    RPM=0;
+    RPM_old=0;
+}
+/*setTime関数の定義*/
+/*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
+void Ec::setTime(double t)
+{
+    dt=t;
+}
+
+double Ec::getRPM()
+{
+    return RPM;
+}
+
+int Ec::getRev()
+{
+    return rev;
+}
+
+void Ec::changeRPM_th(int th)
+{
+    RPM_th=th;
+}
+
+double Ec::getDistance_mm()
+{
+    if(is_diameter_set == 0) printf("please set diameter\r\n");
+    distance_mm_= count * count_to_distance_mm_;
+    return distance_mm_;
+}
+void Ec::setDiameter_mm(double diameter_mm)
+{
+    diameter_mm_ = diameter_mm;
+    setCountToDistance_mm();
+    is_diameter_set = 1;
+}
+void Ec::setGearRate(double gear_rate){
+    gear_rate_ = gear_rate;
+    setCountToDistance_mm();
+}
+void Ec::setCountToDistance_mm(){
+    count_to_distance_mm_ = 1 / (double)solution * M_pi * diameter_mm_ * gear_rate_;//カウント分解能＊2Pi=角度（rad)。角度*直径/2  距離を整理した式
+}
\ No newline at end of file