Diff: main.cpp

Revision:

2:80a1ed62c307

Parent:

1:42bba20ee253

Child:

3:bae8eb81a9d7

--- a/main.cpp	Mon May 18 12:22:57 2015 +0000
+++ b/main.cpp	Thu May 21 14:38:47 2015 +0000
@@ -1,16 +1,155 @@
 #include "mbed.h"
 #include "btbee.h"
 #include "m3pi_ng.h"
+m3pi robot;
+Timer timer;
+Timer time_wait;
+#define MAX 0.95
+#define MIN 0 
 
-DigitalOut myled(LED1);
+#define P_TERM 5
+#define I_TERM 0
+#define D_TERM 20 
+
+
 
 int main(){
-    
-    while(1){
-        myled = 1;   
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
+   
+   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;
+
+   /* 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);
+   
+  
+   
+   //Check to make sure battery isn't low 
+   if (robot.battery() < 2.4)
+    {timer.stop();
+        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);
+              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]);
     }
    
-}
\ No newline at end of file
+   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(.005-time_wait.read());
+   }
+   
+   
+   
+   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