Diff: main.cpp

Revision:

16:2b437630e29e

Parent:

15:cc5b8a858c6e

Child:

17:a6cb4c42308c

--- a/main.cpp	Wed May 21 12:20:43 2014 +0000
+++ b/main.cpp	Wed May 21 12:59:30 2014 +0000
@@ -1,15 +1,17 @@
 #include "mbed.h"
 #include "m3pi_ng.h"
 
+void turn_around(int array[5],float speed);
+
 m3pi thinggy;
 
 
 int main() {
     
-    float speed = 0.5;
+    float speed = 0.25;
     float correction;
-    float k = -0.7;
-    
+    float k = -0.3;
+    int sensor[5];
     
     thinggy.locate(0,1);
     thinggy.printf("HellzYa");
@@ -19,7 +21,10 @@
     thinggy.sensor_auto_calibrate();
     while(1) {
         
-    
+        thinggy.raw_sensor(sensor);
+   
+        
+        turn_around(sensor, speed);
          // -1.0 is far left, 1.0 is far right, 0.0 in the middle
         float position = thinggy.line_position();
         correction = k*(position);
@@ -28,9 +33,7 @@
         if(speed + correction > 1){
             thinggy.right_motor(0.6);
         }
-        else if(speed + correction <= 0){
-            thinggy.right_motor(0.1);
-        }
+        
         else{
             thinggy.right_motor(speed+correction);
         }
@@ -39,15 +42,59 @@
         if(speed - correction > 1){
             thinggy.left_motor(0.6);
         }
-        else if(speed - correction <= 0){
-            thinggy.left_motor(0.1);
-        }
+       
+        
         else{
             thinggy.left_motor(speed-correction);
         }
-            
-        
+                  
+      }
+}
+
+////-------------------------------------------------------------------------------------////
+////------------------------------TURN AROUND FUNCTION-----------------------------------////
+////-------------------------------------------------------------------------------------////
 
+void turn_around(int array[5],float speed){
+        // Declaring Variables
+        int threshold = 200;
         
-      }
-    }
\ No newline at end of file
+        bool turn = false;
+        //NOT NEEDED : int amt    = 0;
+        // Check to see if all sensors are lower than threshold 
+        
+        while(!turn){
+            
+        int x = 0;
+        
+        for(int i = 1; i < 4; i++){
+            x = x + array[i];
+        }
+        
+        int x_avg = x/3;
+        
+            if(x_avg < threshold){
+                turn = true;
+                thinggy.stop();  
+            }           
+         }
+           
+         while(turn){
+            
+         int y = 0;     
+            thinggy.left_motor(speed);
+            thinggy.right_motor(-speed);
+        
+        for(int i = 1; i < 4; i++){
+            y = y + array[i];
+        }
+        
+        int y_avg = y/3;
+        
+            if(y_avg > threshold){
+                turn = false;  
+            }           
+        break;
+         }
+                    
+        }
\ No newline at end of file