Diff: USBTest.cpp

Revision:

15:62648eb5df9f

Parent:

14:46b19affc4b2

Child:

16:b09cc5b75049

--- a/USBTest.cpp	Fri May 29 11:47:52 2015 +0000
+++ b/USBTest.cpp	Fri May 29 13:19:37 2015 +0000
@@ -47,6 +47,8 @@
     m3pi.cls();
     
     Timer LapTimer;
+    Timer LoopTime;
+    Timer WriteTime;
     char Bat[] = {'V','o','l','t',' ','i','s'};
   m3pi.print(Bat,7);
   wait(0.75);
@@ -93,9 +95,13 @@
     int counter = -1;
     int checkvar = 0;
     double Time1, Time2, Time3, Time4, Time5, TimeAve;
-    vector < float > ZeTimes;
-    vector < vector < float > > Data;
+    vector < float > ZeTimes(2);
+    vector < vector < float > > Data(500);
     float stuff;
+    float junk;
+    double timer;
+    double TimeToWrite;
+    int loopcounter = 0;
    
     
     btbee.printf("\n");
@@ -103,21 +109,13 @@
     
     while (1) {
         
-    //if(m3pi_IN [0] == 0) {
-//        while (m3pi_IN [0] == 0) {
-//    btbee.printf("Obstacle detected. Please remove to continue");
-//    m3pi.stop();
-//    wait(3.0);
-//    }
-//}
+        LoopTime.start();
 
-
-        ZeTimes.clear();
         
        // Get the position of the line.
-        current_pos_of_line = m3pi.line_position();  
+        current_pos_of_line = m3pi.line_position(); 
         
-        ZeTimes.push_back(current_pos_of_line);
+        stuff = current_pos_of_line;
               
         proportional = current_pos_of_line;
         //fprintf(fp, "%f", current_pos_of_line);
@@ -154,18 +152,11 @@
         m3pi.left_motor(left);
         m3pi.right_motor(right);
         
-        ZeTimes.push_back(left);
-        ZeTimes.push_back(right);
         
-        //fprintf(fp, "%f", left);
-//        fprintf(fp, "        ");
-//        
-//        fprintf(fp, "%f", right);
-//        fprintf(fp, "\n");
         
     m3pi.calibrated_sensor(LapTest);
     
-    Data.push_back(ZeTimes);
+    
     
     s1 = LapTest[0];
     s2 = LapTest[1];
@@ -232,7 +223,22 @@
         
         }
         
-    if (counter == 0) {
+        ZeTimes[0]= stuff;
+        ZeTimes[1] = power;
+        Data[loopcounter] = ZeTimes;
+        
+        loopcounter++;
+
+        
+        timer = LoopTime.read();
+        
+        if(timer < 0.005) {
+        wait(0.005 - timer);
+        }
+    timer = 0;
+    LoopTime.reset();
+        
+    if (loopcounter == 500) {
         m3pi.stop();
         m3pi.cls();
         m3pi.locate(0,0);
@@ -244,7 +250,9 @@
         btbee.printf("\n");
         btbee.printf("Printing to USB");
         
-        DIR *d;
+   WriteTime.start();
+        
+            DIR *d;
     struct dirent *p;
    
     d = opendir("/" FSNAME);
@@ -254,30 +262,37 @@
     {
         m3pi.leds(1);
     }
-     
-    for( int j = 0; j < 3; j++ ) {
+    
+    for(int i = 0; i < 500; i++) {
         
-        for( int i = 0; i < Data.size(); i++) {
+        for( int j = 0; j < 2; j++)  {
             
-                stuff = Data[i][j];
-        
-    fprintf(fp,"%f \n", stuff); //message
+            junk = Data[i][j];
+            
+            fprintf(fp,"%f        ", junk);
             
             }
+            
+        fprintf(fp,"\r\n");
         
-   fprintf(fp,"                                                                              ");
+        }
     
-    }
-     
-    fclose(fp); 
+   TimeToWrite = WriteTime.read();
+   btbee.printf("%f    \n", TimeToWrite);
+   WriteTime.reset();
     
     btbee.printf("Printed");
+    m3pi.cls();
+    m3pi.printf("Printed");
+    
+    fclose(fp);
         
         
         exit(1);
         }
 
 
+
     }
     
 }