Diff: main.cpp

Revision:

6:99d09e88924b

Parent:

5:c38929c0fd95

Child:

7:7d491b51665e

--- a/main.cpp	Fri May 22 15:07:30 2015 +0000
+++ b/main.cpp	Wed May 27 12:02:28 2015 +0000
@@ -17,9 +17,9 @@
 
 int main()
 {
-    int P_TERM = 5;
-    int I_TERM = 0;
-    int D_TERM = 20;
+    float P_TERM = 1;
+    float I_TERM = 0;
+    float D_TERM = 20;
 
     btbee.reset();
     robot.sensor_auto_calibrate();
@@ -46,12 +46,14 @@
 
     /* for (int i = 0; i <5; ++i)
          current_pos[i] = 0.0; */
-    timer.start();
+    
+    
+    wait(8);
+    btbee.printf("Battery: %f\n", robot.battery());
+    //timer.start();
 
 
     time_wait.start();
-
-    wait(8);
     while(y) {
         time_wait.reset();
         //Get raw sensor values
@@ -63,6 +65,7 @@
         //Check to make sure battery isn't low
         if (robot.battery() < 2.4) {
             timer.stop();
+            btbee.printf("Battery too low\n");
             break;
         }
 
@@ -79,6 +82,7 @@
             }
 
             else if (lap == 2) {
+                robot.stop();
                 lap_time = timer.read();
                 total_time += lap_time;
                 average_time = total_time/lap;
@@ -87,20 +91,25 @@
                     btbee.printf("Lap %d time: %f\n", lap, lap_time);
                     btbee.printf("Avg Lap time: %f\n", average_time);
                 }
-                robot.stop();
+                
                 while (count < 3){
                     //btbee.printf("Input parameter\n");
                     btbee.read_line(arr_read, 30, &chars_read);
                     paramChange[count] = atoi(arr_read);
-                    btbee.printf("%d", arr_read);
+                    //btbee.printf("%d", arr_read);
                     count++;
                 }
                 P_TERM = paramChange[0];
                 I_TERM = paramChange[1];
                 D_TERM = paramChange[2];
+                btbee.printf("PTERM %f\n", P_TERM);
+                btbee.printf("ITERM %f\n", I_TERM);
+                btbee.printf("DTERM %f\n", D_TERM);
                 lap = 0;
                 total_time = 0;
                 count = 0;
+                timer.stop();
+                timer.reset();
                 continue;
                 
             } else {