Diff: EC/EC.cpp

Revision:

0:cca61e773cbb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EC/EC.cpp	Fri Apr 03 02:52:21 2020 +0000
@@ -0,0 +1,137 @@
+#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(this,&Ec::upA);
+        signalA_.fall(this,&Ec::downA);
+        signalB_.rise(this,&Ec::upB);
+        signalB_.fall(this,&Ec::downB);
+    }
+    if(signalZ!=NC){
+        first=false; rev=0; old_time=0; RPM=0; RPM_old=0;
+        signalZ_.rise(this,&Ec::upZ);
+        signalZ_.fall(this,&Ec::downZ);
+    }
+    dt=t;
+    solution=s;
+    C_ec=1/(dt*solution);
+    defsolution=s;
+    RPM_th=250;
+}
+
+int Ec::getCount(){
+    return count;
+}
+
+void Ec::CalOmega(){
+    omega=(count+S/4.0-pre_count)*2*M_pi*C_ec;
+    pre_count=count+S/4.0;
+}
+
+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;
+}
\ No newline at end of file