Diff: main.cpp

Revision:

5:c38929c0fd95

Parent:

4:acd0f86ed832

Child:

6:99d09e88924b

--- a/main.cpp	Fri May 22 14:21:22 2015 +0000
+++ b/main.cpp	Fri May 22 15:07:30 2015 +0000
@@ -7,169 +7,191 @@
 Timer timer;
 Timer time_wait;
 #define MAX 0.95
-#define MIN 0 
+#define MIN 0
 
 //#define P_TERM 5
 //#define I_TERM 0
-//#define D_TERM 20 
+//#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];
-   float current_pos = 0.0;
-   float previous_pos =0.0;
-   float derivative, proportional, integral = 0; 
-   float power;
-   float speed = MAX;
-   
-   int lap = 0;
-   float lap_time = 0.0;
-   float total_time = 0.0;
-   float average_time = 0.0;
-  int y =1;
-  
+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];
+    float current_pos = 0.0;
+    float previous_pos =0.0;
+    float derivative, proportional, integral = 0;
+    float power;
+    float speed = MAX;
+
+    int lap = 0;
+    float lap_time = 0.0;
+    float total_time = 0.0;
+    float average_time = 0.0;
+    int y =1;
+    int count = 0;
+    int paramChange[3];
+
     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; */
-   timer.start(); 
-   
+    /* for (int i = 0; i <5; ++i)
+         current_pos[i] = 0.0; */
+    timer.start();
+
+
+    time_wait.start();
+
+    wait(8);
+    while(y) {
+        time_wait.reset();
+        //Get raw sensor values
+        int x [5];
+        robot.calibrated_sensor(x);
+
+
+
+        //Check to make sure battery isn't low
+        if (robot.battery() < 2.4) {
+            timer.stop();
+            break;
+        }
+
+        //else if (m3pi_IN [0] == 0)
+        //{break;}
+
+        else if( x[0] > 300 && x[2]>300 && x[4]>300) {
+            if (lap == 0) {
+                while( x[0]> 300 && x[4] > 300) {
+                    robot.calibrated_sensor(x);
+                }
+                timer.start();
+                lap= lap +1;
+            }
+
+            else if (lap == 2) {
+                lap_time = timer.read();
+                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);
+                }
+                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);
+                    count++;
+                }
+                P_TERM = paramChange[0];
+                I_TERM = paramChange[1];
+                D_TERM = paramChange[2];
+                lap = 0;
+                total_time = 0;
+                count = 0;
+                continue;
+                
+            } 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;
+                timer.reset();
+            }
+        }
+
 
-   time_wait.start(); 
-   
-   wait(8);
-   while(y)
-   {time_wait.reset();
-   //Get raw sensor values
-   int x [5];
-    robot.calibrated_sensor(x);
-   
-  
-   
-   //Check to make sure battery isn't low 
-   if (robot.battery() < 2.4)
-    {timer.stop();
-        break;}
-        
-    //else if (m3pi_IN [0] == 0)
-    //{break;}
-   
-    else if( x[0] > 300 && x[2]>300 && x[4]>300)
-            { if (lap == 0)
-            {   while( x[0]> 300 && x[4] > 300)
-            {robot.calibrated_sensor(x);}
-            timer.start(); 
-              lap= lap +1;     
-             }
-                         
-             else if (lap == 5)
-             {lap_time = timer.read();
-             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;
-             timer.reset(); }  
-             }
-       
-       
-       // Get the position of the line.
-   /*  for (int i =0; i < 4; ++i)
-        current_pos[i] = current_pos[i+1];  
-      current_pos[4] = robot.line_position();   
-   proportional = current_pos[4]; 
-   
-   // compute the derivative 
-   derivative = 0;
-   for (int i =1; i<5;++i) {
-        if (i ==1)
-            derivative += 0*(current_pos[i] - current_pos[i-1]);
-         else if (i == 2)
-            derivative += 0*(current_pos[i] - current_pos[i-1]);  
-         else if (i==3)
-            derivative += 0*(current_pos[i] - current_pos[i-1]);    
-        else
-            derivative += (current_pos[i] - current_pos[i-1]);
+        // Get the position of the line.
+        /*  for (int i =0; i < 4; ++i)
+             current_pos[i] = current_pos[i+1];
+           current_pos[4] = robot.line_position();
+        proportional = current_pos[4];
+
+        // compute the derivative
+        derivative = 0;
+        for (int i =1; i<5;++i) {
+             if (i ==1)
+                 derivative += 0*(current_pos[i] - current_pos[i-1]);
+              else if (i == 2)
+                 derivative += 0*(current_pos[i] - current_pos[i-1]);
+              else if (i==3)
+                 derivative += 0*(current_pos[i] - current_pos[i-1]);
+             else
+                 derivative += (current_pos[i] - current_pos[i-1]);
+         }
+
+        derivative = derivative; */
+
+
+        current_pos = robot.line_position();
+        proportional = current_pos;
+
+        derivative = current_pos - previous_pos;
+
+
+        //compute the integral
+        integral =+ proportional;
+
+        //remember the last position.
+        previous_pos = current_pos;
+
+        // compute the power
+        power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+        //computer new speeds
+        right = speed+power;
+        left = speed-power;
+
+        //limit checks
+        if(right<MIN)
+            right = MIN;
+        else if (right > MAX)
+            right = MAX;
+
+        if(left<MIN)
+            left = MIN;
+        else if (left>MIN)
+            left = MAX;
+
+        //set speed
+
+        robot.left_motor(left);
+        robot.right_motor(right);
+
+        wait((5-time_wait.read_ms())/1000);
     }
-   
-   derivative = derivative; */
-   
-   
-   current_pos = robot.line_position();
-   proportional = current_pos;
-   
-   derivative = current_pos - previous_pos;
-   
-   
-   //compute the integral
-   integral =+ proportional;
-   
-   //remember the last position.
-   previous_pos = current_pos;
-   
-   // compute the power 
-  power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
-   //computer new speeds    
-  right = speed+power;
-  left = speed-power; 
-   
-   //limit checks
-   if(right<MIN)
-   right = MIN;
-   else if (right > MAX)
-    right = MAX;
-    
-    if(left<MIN)
-    left = MIN;
-    else if (left>MIN)
-    left = MAX;
-   
-   //set speed
-   
-   robot.left_motor(left);
-   robot.right_motor(right);
-   
-   wait((5-time_wait.read_ms())/1000);
-   }
-   
-   
-   
-   robot.stop();
-   
-     char hail[]={'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
-,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
-,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
-,'G','8','E','8','D','8','C','4'};
+
+
+
+    robot.stop();
+
+    char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
+                  ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
+                  ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
+                  ,'G','8','E','8','D','8','C','4'
+                 };
     int numb = 59;
-     
+
     robot.playtune(hail,numb);
-   
-   
-   
-           
-    }
\ No newline at end of file
+
+
+
+
+}
\ No newline at end of file