Diff: main.cpp

Revision:

4:0cdad3a8484b

Parent:

3:bae8eb81a9d7

--- a/main.cpp	Fri May 22 13:26:21 2015 +0000
+++ b/main.cpp	Wed Jun 03 11:45:53 2015 +0000
@@ -3,12 +3,16 @@
 #include "m3pi_ng.h"
 m3pi robot;
 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
+
+
 Timer timer;
 Timer time_wait;
-#define MAX 0.95
+
+
+#define MAX 0.4
 #define MIN 0 
 
-#define P_TERM 5
+#define P_TERM 1
 #define I_TERM 0
 #define D_TERM 20 
 
@@ -21,29 +25,18 @@
    
    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;
 
-   /* for (int i = 0; i <5; ++i)
-        current_pos[i] = 0.0; */
-   timer.start(); 
-   
 
-   time_wait.start(); 
-   
-   
    while(y)
-   {time_wait.reset();
+   {
    //Get raw sensor values
    int x [5];
     robot.calibrated_sensor(x);
@@ -55,63 +48,28 @@
     {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(x[0] > 300 && x[4] > 300) 
+    {  while( x[0]> 300 && x[4] > 300)
+     {robot.calibrated_sensor(x);}
+       y = 0;           
              }
-                         
-             else if (lap == 5)
-             {lap_time = timer.read();
-             total_time += lap_time;
-             average_time = total_time/lap;
-             robot.printf("%f",average_time);
-              y=0; 
-              break;}
-              else
-              {  while( x[0]> 300 && x[4] > 300)
-            {robot.calibrated_sensor(x);}
-             lap_time = timer.read();
-             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]);
-    }
-   
-   derivative = derivative; */
-   
+    else if (x[0]>300 && x[4] < 20)   
+      {} 
+
+    else if (x[0]<20 && x[4] > 300)  
+    { while 
+    
+    
    
    current_pos = robot.line_position();
-   proportional = current_pos;
    
+   //compute the integral
+   proportional = current_pos; 
+   
+   //compute the integral  
    derivative = current_pos - previous_pos;
    
-   
    //compute the integral
    integral =+ proportional;
    
@@ -120,6 +78,7 @@
    
    // compute the power 
   power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+  
    //computer new speeds    
   right = speed+power;
   left = speed-power; 
@@ -146,14 +105,14 @@
    
    
    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);
+  //  robot.playtune(hail,numb);