Diff: main.cpp

Revision:

5:f4c237d0bb32

Parent:

4:f8a45966e63b

Child:

6:73e417b1c521

--- a/main.cpp	Sat Feb 04 21:19:06 2017 +0000
+++ b/main.cpp	Mon Feb 06 20:03:40 2017 +0000
@@ -72,15 +72,13 @@
 }
 
 
-
 int main() {
     
     int endcount, startcount;
     double time_between_readings;
     double velocity;
     double currentSensed = 0;
-    clock_t start;
-    clock_t end = clock();
+    clock_t start, end;
     int ticks;
     a=1; b=0; pwm.write(0);
     button.fall(&pressed);
@@ -100,42 +98,55 @@
     /*** wait here for matlab information like voltage before starting? ***/
     
     while(1) {
-        
-        wait(updatePeriod);
-        start = end;
-        end = clock();        
-        time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
-        startcount = endcount;
-        endcount = GetCounts();       
-        ticks = endcount-startcount;
-        if(abs(ticks)>CPR/2) /***** for rollover case: *****/
-        { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
-        velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
-        
-        currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
-        
-        if(pc.readable())
-        {
-            char charIn = pc.getc();
-            if(charIn == 'r'){
-                fflush(pc);
-                green = !green;
-            } else if(isdigit(charIn)) {
-                double abrahamsCommand = (double)(charIn - '0');
-                pwm.write(abrahamsCommand/10.0);
+    
+        while(1) {
+            green = true;
+            if(pc.readable())
+            {
+                char charIn = pc.getc();
+                if(charIn == 'g'){ end = clock(); break; }
             }
-        }
-                    
-        if(publishCounter == samplesPerPublish)
+            wait(0.05);
+        }  
+    
+    
+        while(1) {
+            green = false;
+            wait(updatePeriod);
+            clock_t absoluteStart = clock();
+            start = end;
+            end = clock();        
+            time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
+            startcount = endcount;
+            endcount = GetCounts();       
+            ticks = endcount-startcount;
+            if(abs(ticks)>CPR/2) /***** for rollover case: *****/
+            { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
+            velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
+            currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
+            if(pc.readable())
             {
-            printf("%f,%f,%f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity);
-            publishCounter = 1;
-            }
-        publishCounter++;
-        
+                char charIn = pc.getc();
+                if(charIn == 'r'){
+                    fflush(pc); /* TODO: purge much better than this! */
+//                    green = !green;
+                    break;
+                } else if(isdigit(charIn)) {
+                    double abrahamsCommand = (double)(charIn - '0');
+                    pwm.write(abrahamsCommand/10.0);
+                }
+            }        
+            if(publishCounter == samplesPerPublish)
+                {
+                printf("%+8f,%+8f,%+8f,%+8f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC));
+                publishCounter = 1;
+                }
+            publishCounter++;
+            
+        }
+    
     }
     
-    
 }