Diff: EC.cpp

Revision:

32:297384f9d261

Parent:

26:45a53e3c81b1

Child:

34:b34dc495b3c8

--- a/EC.cpp	Thu Aug 17 07:49:21 2017 +0000
+++ b/EC.cpp	Wed Jul 04 04:21:37 2018 +0000
@@ -4,7 +4,8 @@
 int Ec::defsolution;
 double Ec::deftime;
 //ピン変化割り込み関数の定義
-void Ec::upA(){
+void Ec::upA()
+{
     stateA=1;
     if(stateB==0&&S==0) {
         S=1;
@@ -12,7 +13,8 @@
         S=2;
     }
 }
-void Ec::downA(){
+void Ec::downA()
+{
     stateA=0;
     if(stateB==1&&S==2) {
         S=3;
@@ -21,7 +23,8 @@
         count--;
     }
 }
-void Ec::upB(){
+void Ec::upB()
+{
     stateB=1;
     if(stateA==1&&S==1) {
         S=2;
@@ -29,7 +32,8 @@
         S=3;
     }
 }
-void Ec::downB(){
+void Ec::downB()
+{
     stateB=0;
     if(stateA==0&&S==3) {
         count++;
@@ -39,30 +43,32 @@
     }
 }
 
-void Ec::upZ(){
-    if(first==false){
+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){
+        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{
+            } else {
                 res_count++;
             }
-        }else{
+        } else {
             res_count=0;
         }
         RPM_old=RPM;
         old_time=timer.read();
-    }    
+    }
 }
 
-void Ec::downZ(){
+void Ec::downZ()
+{
     first=false;
 }
 
@@ -74,63 +80,108 @@
 
 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;
+
+    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);
+        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(this,&Ec::upZ);
-        signalZ_.fall(this,&Ec::downZ);
+    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;
 }
 
-int Ec::getCount(){
+int Ec::getCount()
+{
     return count;
 }
 
-void Ec::CalOmega(){
+void Ec::CalOmega()
+{
     omega=(count-pre_count)*2*M_pi/(solution*dt);
     pre_count=count;
 }
 
-double Ec::getOmega(){
+double Ec::getOmega()
+{
     return omega;
 }
 
-double Ec::getPreCount(){
+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;
+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){
+void Ec::setTime(double t)
+{
     dt=t;
 }
 
-double Ec::getRPM(){
+double Ec::getRPM()
+{
     return RPM;
 }
 
-int Ec::getRev(){
+int Ec::getRev()
+{
     return rev;
 }
 
-void Ec::changeRPM_th(int th){
+void Ec::changeRPM_th(int th)
+{
     RPM_th=th;
+}
+
+double Ec::getDistance_mm()
+{
+    distance_mm_= count * count_to_distance_mm_;
+    return distance_mm_;
+}
+void Ec::setDiameter_mm(double diameter_mm)
+{
+    diameter_mm_ = diameter_mm;
+    setCountToDistance_mm();
+}
+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