Diff: main.cpp

Revision:

5:acd0f86ed832

Parent:

3:bae8eb81a9d7

Child:

6:c38929c0fd95

--- a/main.cpp	Fri May 22 13:26:21 2015 +0000
+++ b/main.cpp	Fri May 22 14:21:22 2015 +0000
@@ -2,23 +2,27 @@
 #include "btbee.h"
 #include "m3pi_ng.h"
 m3pi robot;
+btbee btbee;
 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
 Timer timer;
 Timer time_wait;
 #define MAX 0.95
 #define MIN 0 
 
-#define P_TERM 5
-#define I_TERM 0
-#define D_TERM 20 
+//#define P_TERM 5
+//#define I_TERM 0
+//#define D_TERM 20 
 
 
 
 int main(){
+   int P_TERM = 5;
+   int I_TERM = 0;
+   int D_TERM = 20;
    
+   btbee.reset();
    robot.sensor_auto_calibrate();
    wait(2.0);
-   
    float right;
    float left;
    //float current_pos[5];
@@ -33,6 +37,9 @@
    float total_time = 0.0;
    float average_time = 0.0;
   int y =1;
+  
+    char arr_read[30]; // this should be long enough to store any reply coming in over bt.
+    int  chars_read;
 
    /* for (int i = 0; i <5; ++i)
         current_pos[i] = 0.0; */
@@ -41,7 +48,7 @@
 
    time_wait.start(); 
    
-   
+   wait(8);
    while(y)
    {time_wait.reset();
    //Get raw sensor values
@@ -55,8 +62,8 @@
     {timer.stop();
         break;}
         
-    else if (m3pi_IN [0] == 0)
-    {break;}
+    //else if (m3pi_IN [0] == 0)
+    //{break;}
    
     else if( x[0] > 300 && x[2]>300 && x[4]>300)
             { if (lap == 0)
@@ -71,12 +78,19 @@
              total_time += lap_time;
              average_time = total_time/lap;
              robot.printf("%f",average_time);
+             if (btbee.writeable()){
+                 btbee.printf("Lap %d time: %f\n", lap, lap_time);
+                 btbee.printf("Avg Lap time: %f\n", average_time);
+             }
               y=0; 
               break;}
               else
               {  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);
+             }
              total_time += lap_time;
              average_time = total_time/lap;
              lap = lap +1;