Diff: QEI.cpp

Revision:

5:4682b3d415bd

Parent:

4:9699a757d4ed

--- a/QEI.cpp	Tue Oct 25 13:05:41 2016 +0000
+++ b/QEI.cpp	Thu Mar 30 07:53:34 2017 +0000
@@ -174,9 +174,14 @@
     if (index !=  NC) {
         index_.rise(this, &QEI::index);
     }
-    // Parameters for unit changing
-    count_2_rad_s = 1/(pulsesPerRev_*1.0)*6.2831852/sampletime_/(arraysize_*1.0);
-    count_2_deg_s = 1/(pulsesPerRev_*1.0)*360.0/sampletime_/(arraysize_*1.0);
+
+    // Unit transformation
+    // Rotational speed
+    count_2_rad_s = 1.0/(pulsesPerRev_*1.0)*6.2831852/sampletime_/(arraysize_*1.0);
+    count_2_deg_s = 1.0/(pulsesPerRev_*1.0)*360.0/sampletime_/(arraysize_*1.0);
+    // Angle
+    count_2_rad = 1.0/(pulsesPerRev_*1.0)*6.2831852;
+    count_2_deg = 1.0/(pulsesPerRev_*1.0)*360.0;
 }
 
 void QEI::reset(void) {
@@ -280,7 +285,7 @@
 
         //Entered a new valid state.
         if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
-            // 2 bit state. 
+            // 2 bit state.
             // (Right hand bit of prev) XOR (left hand bit of current)
             // B_(n-1) xor A_n (because of the 90deg phase shift and A is leading B when rotates clockwise)
             // gives 0 if clockwise rotation and 1 if counter clockwise rotation.
@@ -309,7 +314,7 @@
 void QEI::Calculate(){                      // Moving average: calculating the speed of the motor for feedback control
     // new input
     countArray[arrayPtr] = pulses_;
-    
+
     // nextPtr: Actually, it is the oldest one.
     uint8_t nextPtr = arrayPtr + 1;
     if(nextPtr > (arraysize_ - 1) ){nextPtr = 0;}
@@ -326,4 +331,27 @@
 
 double QEI::getAngularSpeed_deg_s(){
     return angularvelocity*count_2_deg_s;
-}
\ No newline at end of file
+}
+// Angle
+double QEI::getAngle(bool is_ranged){ // rad, if is_ranged, return 0~2*PI
+    //
+    int pulse_temp = this->pulses_;
+    //
+    if (is_ranged){
+        revolutions_ = pulse_temp/pulsesPerRev_;
+        return (pulse_temp % pulsesPerRev_)*count_2_rad;
+    }else{
+        return (pulse_temp*count_2_rad);
+    }
+}
+double QEI::getAngle_deg(bool is_ranged){ // deg, if is_ranged, return 0~360
+    //
+    int pulse_temp = this->pulses_;
+    //
+    if (is_ranged){
+        revolutions_ = pulse_temp/pulsesPerRev_;
+        return (pulse_temp % pulsesPerRev_)*count_2_deg;
+    }else{
+        return (pulse_temp*count_2_deg);
+    }
+}