Diff: main.cpp

Revision:

17:a6cb4c42308c

Parent:

16:2b437630e29e

Child:

18:eddba761c425

--- a/main.cpp	Wed May 21 12:59:30 2014 +0000
+++ b/main.cpp	Wed May 21 14:42:33 2014 +0000
@@ -1,8 +1,14 @@
 #include "mbed.h"
 #include "m3pi_ng.h"
+#include "cmath"
+#include "iostream"
+
+using namespace std;
 
 void turn_around(int array[5],float speed);
 
+int sensor_refresh(m3pi thinggy, int sensor[5], int x);
+
 m3pi thinggy;
 
 
@@ -12,28 +18,52 @@
     float correction;
     float k = -0.3;
     int sensor[5];
-    
+    int returned;   
     thinggy.locate(0,1);
-    thinggy.printf("HellzYa");
+    thinggy.printf("Villan");
+    thinggy.locate(0,0);
+    thinggy.printf("Pimpin");
     
     wait(1.0);
-    
+ 
     thinggy.sensor_auto_calibrate();
+ 
+    thinggy.calibrated_sensor(sensor);
+    returned = sensor[2];
+    thinggy.cls();
+    thinggy.print("Apple",5);
+    
+    
     while(1) {
+        thinggy.print("Apfel",5);
+        // change "if" to while
+        while(returned < 200){
+                while(sensor[0] < sensor[4]){
+                    thinggy.left_motor(speed);
+                    thinggy.right_motor(-speed);
+                    thinggy.calibrated_sensor(sensor);
+                }
+                while(sensor[4] > sensor[0]){
+                    thinggy.left_motor(-speed);
+                    thinggy.right_motor(speed);
+                    thinggy.calibrated_sensor(sensor);
+                }
+                thinggy.calibrated_sensor(sensor);
+                returned = sensor[2];
+        }
         
-        thinggy.raw_sensor(sensor);
-   
-        
-        turn_around(sensor, speed);
-         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+        // change "if" to while    
+        while(returned > 200){
+ 
         float position = thinggy.line_position();
         correction = k*(position);
+       
+         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
         
         //speed limiting for right motor
         if(speed + correction > 1){
             thinggy.right_motor(0.6);
-        }
-        
+        } 
         else{
             thinggy.right_motor(speed+correction);
         }
@@ -42,59 +72,17 @@
         if(speed - correction > 1){
             thinggy.left_motor(0.6);
         }
-       
-        
         else{
             thinggy.left_motor(speed-correction);
         }
-                  
-      }
+         thinggy.calibrated_sensor(sensor);
+         returned = sensor[2];   
+        }
+    thinggy.calibrated_sensor(sensor);
+    returned = sensor[2];
+    }
 }
 
-////-------------------------------------------------------------------------------------////
-////------------------------------TURN AROUND FUNCTION-----------------------------------////
-////-------------------------------------------------------------------------------------////
+
 
-void turn_around(int array[5],float speed){
-        // Declaring Variables
-        int threshold = 200;
-        
-        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
+