Diff: main.cpp

Revision:

14:168aa965b1b4

Parent:

13:582064cfb9b2

--- a/main.cpp	Thu Mar 13 16:59:09 2014 +0000
+++ b/main.cpp	Wed Mar 19 18:44:49 2014 +0000
@@ -140,6 +140,8 @@
     int mTooFarOffFor = 0;
     const int MAX_TOO_FAR_OFF_COUNT = 2;
     
+    long thisMs = 0, lastMs = 0;
+    
     while(1) {
         if (pc.readable()) {
             processSerial();
@@ -214,21 +216,24 @@
                 awaitingPackets = true;
             }
         }
-
+        // dilemma - else or no else?
         if (expectingAck1) {
             expectingAck1 = !getPacket(0);
-        } else if (expectingAck2) {
+        } 
+        else if (expectingAck2) {
             expectingAck2 = !getPacket(1);
-        } else if (expectingAck3) {
+        }
+        else if (expectingAck3) {
             expectingAck3 = !getPacket(2);
         }
         // TODO only prints when both are enabled now
         if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
+            thisMs = timer.read_ms();
             if ((sensorXs[0] | sensorYs[0] | sensorXs[1] | sensorYs[1] | sensorXs[2] | sensorYs[2]) ) {
                 // some of the velocities are not 0
                 if (notTooFarOff()) {
-                    printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
-                       sensorXs[2], sensorYs[2]);
+                    printf("%d : %d %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
+                       sensorXs[2], sensorYs[2], thisMs - lastMs);
                 }
                 else {
                     mTooFarOffFor++;
@@ -246,6 +251,7 @@
                     }
                 }
             }
+            lastMs = thisMs;
             mTooFarOffFor = 0;
             sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
             sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;