Diff: main.cpp

Revision:

6:ed11342ab079

Parent:

5:36788b154e25

Child:

7:e7aa4f10d1fb

diff -r 36788b154e25 -r ed11342ab079 main.cpp
--- a/main.cpp	Fri Oct 07 12:06:16 2016 +0000
+++ b/main.cpp	Fri Oct 07 12:26:22 2016 +0000
@@ -9,6 +9,12 @@
 int countsCCW = 0;
 int net_counts = 0;
 float degrees = 0.0;
+
+volatile float curr_degrees = 0.0;
+volatile float prev_degrees = 0.0;
+volatile float speed = 0.0;          // speed in degrees/s
+volatile const float T_CalculateSpeed = 0.1; // 100 Hz
+
 const float counts_per_rev = 4200.0;
 QEI EncoderCW(D12,D13,NC,32);
 QEI EncoderCCW(D13,D12,NC,32);
@@ -16,7 +22,14 @@
 void PrintDegrees(){
     pc.printf("\r\n Nett Pulses %i \r\n", net_counts);
     pc.printf("\r\n Output degrees  %f \r\n", degrees);
+    pc.printf("\r\n Speed %f \r\n",speed);
     }
+    
+void CalculateSpeed() {
+    curr_degrees = degrees;
+    speed = (curr_degrees-prev_degrees)/T_CalculateSpeed; 
+    prev_degrees = curr_degrees;
+}
 
 int main()
 {
@@ -27,6 +40,10 @@
     Ticker PrintDegreesTicker;
     PrintDegreesTicker.attach(&PrintDegrees,0.1);
     
+    // Set ticker for speed calculation
+    Ticker CalculateSpeedTicker;
+    CalculateSpeedTicker.attach(CalculateSpeed,T_CalculateSpeed);
+    
     // count the CW and CCW counts and calculate the output degrees
     while(true){
     countsCW = EncoderCW.getPulses();