Diff: main.cpp

Revision:

8:bd2f012e2f57

Parent:

7:7d491b51665e

Child:

9:030b7e4ff7be

--- a/main.cpp	Wed May 27 13:36:42 2015 +0000
+++ b/main.cpp	Thu May 28 13:40:16 2015 +0000
@@ -1,12 +1,20 @@
 #include "mbed.h"
+#include "MSCFileSystem.h"
 #include "btbee.h"
 #include "m3pi_ng.h"
+#include <fstream>
+#define FSNAME "msc"
+
+
+MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
+Serial pc(USBTX,USBRX);
+
 m3pi robot;
 btbee btbee;
 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
 Timer timer;
 Timer time_wait;
-#define MAX 0.95
+#define MAX 1
 #define MIN 0
 
 //#define P_TERM 5
@@ -17,8 +25,8 @@
 
 int main()
 {
-    float P_TERM = 1;
-    float I_TERM = 0;
+    float P_TERM = 2.5;
+    float I_TERM = .5;
     float D_TERM = 20;
 
     btbee.reset();
@@ -40,9 +48,18 @@
     int y =1;
     int count = 0;
     float paramChange[3];
+    bool passed = false;
 
     char arr_read[30]; // this should be long enough to store any reply coming in over bt.
     int  chars_read;
+    
+    int check = msc.disk_initialize();
+    
+    robot.locate(0,0);
+    robot.printf("USBWrite"); 
+    robot.locate(0,1);
+    robot.printf("Test"); 
+    ofstream myFile ("/" FSNAME "/data.txt"); 
 
     /* for (int i = 0; i <5; ++i)
          current_pos[i] = 0.0; */
@@ -72,16 +89,16 @@
         //else if (m3pi_IN [0] == 0)
         //{break;}
 
-        if( x[0] > 300 && x[2]>300 && x[4]>300) {
+        if( x[0] > 300 && x[2]>300 && x[4]>300 & !passed) {
             if (lap == 0) {
-                while( x[0]> 300 && x[4] > 300) {
+                /*while( x[0]> 300 && x[4] > 300) {
                     robot.calibrated_sensor(x);
-                }
+                }*/
                 timer.start();
                 lap= lap +1;
             }
 
-            else if (lap == 2) {
+            else if (lap == 5) {
                 robot.stop();
                 lap_time = timer.read();
                 total_time += lap_time;
@@ -113,9 +130,9 @@
                 continue;
                 
             } else {
-                while( x[0]> 300 && x[4] > 300) {
+                /*while( x[0]> 300 && x[4] > 300) {
                     robot.calibrated_sensor(x);
-                }
+                }*/
                 lap_time = timer.read();
                 if (btbee.writeable()) {
                     btbee.printf("Lap %d time: %f\n", lap, lap_time);
@@ -125,7 +142,13 @@
                 lap = lap +1;
                 timer.reset();
             }
+            passed = true;
         }
+        else if (x[0] > 300 && x[2]>300 && x[4]>300)
+            passed = true;
+        else
+            passed = false;
+        
 
 
         // Get the position of the line.
@@ -183,6 +206,19 @@
 
         robot.left_motor(left);
         robot.right_motor(right);
+        
+        
+        if (myFile.is_open())
+        {
+            myFile << left << " " << right << " " << robot.line_position(); 
+            robot.cls(); 
+            robot.locate(0,0); 
+            robot.printf("Done."); 
+        }
+        else
+        {
+            robot.printf("Error."); 
+        }
 
         wait((5-time_wait.read_ms())/1000);
     }