Diff: main.cpp

Revision:

1:a33723b70582

Parent:

0:398432a37ca5

Child:

2:088eeae4287c

--- a/main.cpp	Tue Aug 05 11:23:39 2014 +0000
+++ b/main.cpp	Tue Aug 05 13:09:58 2014 +0000
@@ -22,6 +22,8 @@
 int direction = 0;
 double position = 0.0;
 double currentPower = 0.0;
+int interruptPeriod = 0;
+int lastTime = 0;
 
 char counter = 0;
 
@@ -39,6 +41,8 @@
         {
             direction = 1;
             position += 0.04 * direction;
+            interruptPeriod = RunningTime.read_us() - lastTime;
+            lastTime = RunningTime.read_us();
         }
     }
     else
@@ -61,6 +65,8 @@
         {        
             direction = -1;
             position += 0.04 * direction;
+            interruptPeriod = RunningTime.read_us() - lastTime;
+            lastTime = RunningTime.read_us();
         }
     }
     else
@@ -139,11 +145,21 @@
     }
 }
 
+double GetSpeed()
+{
+    if ((RunningTime - lastTime) > 10000)
+    {
+        return 0.0;
+    }
+    return (direction * 0.04)/((double)interruptPeriod / 1000000.0);
+}
+
 void SerialTransmit()
 {        
     double tempPos = position;
     double tempTime = RunningTime.read();
     double tempPow = currentPower;
+    double tempSpeed = GetSpeed();
     int outChar = 0;
     
     if (tempPos < 0.0)
@@ -299,6 +315,58 @@
         pc.putc(outChar + 48);
     }
     
+    pc.putc(',');
+    outChar = 0;
+    
+    if (tempSpeed < 0.0)
+    {
+        pc.putc('-');
+        tempSpeed *= -1;
+    }
+    if (tempSpeed >= 100.0)
+    {
+        outChar = tempSpeed / 100;
+        pc.putc(outChar + 48);
+        tempSpeed -= outChar * 100.0;
+    }
+    if (tempSpeed >= 10.0)
+    {
+        outChar = tempSpeed / 10;
+        pc.putc(outChar + 48);
+        tempSpeed -= outChar * 10.0;
+    }
+    else if(outChar > 0)
+    {
+        pc.putc('0');
+    }
+    if (tempSpeed >= 1.0)
+    {
+        outChar = tempSpeed;
+        pc.putc(outChar + 48);
+        tempSpeed -= outChar;
+    }
+    else
+    {
+        pc.putc('0');
+    }
+    if (tempSpeed >= 0.1)
+    {
+        pc.putc('.');
+        outChar = tempSpeed * 10;
+        pc.putc(outChar + 48);
+        tempSpeed -= (double)outChar / 10.0;
+    }
+    else
+    {
+        pc.putc('.');
+        pc.putc('0');
+    }
+    if (tempSpeed >= 0.01)
+    {
+        outChar = tempSpeed * 100;
+        pc.putc(outChar + 48);
+    }
+    
     pc.putc(10);
     pc.putc(13);
 }