Diff: main.cpp

Revision:

2:b5031bb5303e

Parent:

1:4f52a001926a

Child:

3:bac13ce5f5d0

--- a/main.cpp	Thu May 22 14:52:13 2014 +0000
+++ b/main.cpp	Mon May 26 07:40:29 2014 +0000
@@ -8,6 +8,8 @@
 DigitalOut led_1(p13);
 
 using namespace std;
+
+void cross_detection(int sensor[5], int black_thresh, int white_thresh); 
  
 m3pi thinggy; 
  
@@ -19,6 +21,8 @@
     float k = -0.3;
     int sensor[5];
     int returned;   
+    int black_thresh = 300;
+    int white_thresh = 240;  
     thinggy.locate(0,1);
     thinggy.printf("Villan");
     thinggy.locate(0,0);
@@ -35,8 +39,11 @@
  
     while(1) {
         
+        cross_detection(sensor, black_thresh, white_thresh); 
+        
         //check if it needs to turn  
         while(returned <= 240){
+                cross_detection(sensor, black_thresh, white_thresh);
                 //turns right 
                 while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
                     thinggy.left_motor(turn_speed);
@@ -55,6 +62,9 @@
         
         // Curves and straightaways    
         while(returned > 240){
+            
+            cross_detection(sensor, black_thresh, white_thresh); 
+        
             float position = thinggy.line_position();
             correction = k*(position);
        
@@ -85,7 +95,35 @@
             returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;   
         }//while returned > 220
         
+        thinggy.calibrated_sensor(sensor);
+        returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
     }//while(1)
+    
+
 }
  
+ //REQUIRES: array of 5 ints 
+ //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
+ void cross_detection(int sensor[5], int black_thresh, int white_thresh){
+       //three directions to choose from NOT WORKING 
+        if(sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] > black_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh){
+            thinggy.stop();
+            wait(300);  
+        }
+        //left or forward 
+        else if(sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] > black_thresh and sensor[3] < white_thresh and sensor[4] < white_thresh){
+            thinggy.stop(); 
+            wait(300); 
+        }
+        //left or right WORKING
+        else if (sensor[0] > black_thresh and sensor[1] < white_thresh and sensor[2] < white_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh ){
+            thinggy.stop();
+            wait(300);  
+        } 
+        //forward or right
+        else if (sensor[0] < white_thresh and sensor[1] < white_thresh and sensor[2] > black_thresh and sensor[3] < white_thresh and sensor[4] > black_thresh){
+            thinggy.stop(); 
+            wait(300); 
+        } 
+}
  
\ No newline at end of file