Diff: encoder.cpp

Revision:

0:4b99060621fd

Child:

1:c4643cba43a9

diff -r 000000000000 -r 4b99060621fd encoder.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.cpp	Mon Jun 04 07:38:05 2018 +0000
@@ -0,0 +1,68 @@
+#include "encoder.h"
+
+Encoder::Encoder(PinName aPhase , PinName bPhase ) : aPhase(aPhase) , bPhase(bPhase) {}
+
+void Encoder::calState(){
+    //パルスの差分を更新
+    count[DELTA][ONE_BEFORE] = count[ONE_BEFORE] - count[TWO_BEFORE];
+    count[DELTA][NOW]        = count[NOW] - count[ONE_BEFORE];
+    
+    distance = count[NORMAL][NOW] * circle / ppr;
+    rpm = (count[DELTA][NOW]/ppr) * 60 / dt;
+    velocity = (count[DELTA][NOW]/ppr)*circle/dt;
+    acceleration = (count[DELTA][NOW]-count[DELTA][ONE_BEFORE])/(dt_square*ppr) * circle;
+    omega = (2*PI*count[DELTA][NOW])/(ppr*dt);
+    delta_distance = (count[DELTA][NOW]/ppr)*circle;
+    //1つ前・2つ前のデータを更新
+    count[NORMAL][TWO_BEFORE] = count[NORMAL][ONE_BEFORE];
+    count[NORMAL][ONE_BEFORE] = count[NORMAL][NOW];
+}
+
+float Encoder::getState(int ch){
+    float output;
+    switch(ch){
+        case DISTANCE:
+            output = distance;
+            break;
+        case RPM:
+            distance = rpm;
+            break;
+        case VELOCITY:
+            output = velocity;
+            break;
+        case ACCELERATION:
+            output = acceleration;
+            break;
+        case OMEGA:
+            output = omega;
+            break;
+        case DELTA_DISTANCE:
+            output = delta_distance;
+            break;
+    }
+    return output;
+}
+
+void Encoder::reset(){
+    for(int i=NOW;i<BEFORE_NUMBER;i++){
+        count[NORMAL][i] = 0;
+        count[DELTA][i]  = 0;   
+    }
+}
+
+void Encoder::read(){
+    if(bPhase){
+        count[NORMAL][NOW]++; 
+    } else {
+        count[NORMAL][NOW]--; 
+    }
+}
+
+void Encoder::init(float _ppr, float _radius,float _dt){
+    aPhase.rise(this,&Encoder::read);
+    ppr     = _ppr;
+    dt      = _dt;
+    dt_square = dt*dt;
+    circle  = PI*2.0f*_radius;
+    radius = _radius;
+}