File content as of revision 2:b5031bb5303e:

#include "mbed.h"
#include "m3pi_ng.h"
#include "cmath"
#include "iostream"

//Access infared sensors
DigitalIn m3pi_IN[] = {(p12)};
DigitalOut led_1(p13);

using namespace std;

void cross_detection(int sensor[5], int black_thresh, int white_thresh); 
 
m3pi thinggy; 
 
int main() {
    
    float speed = 0.25;
    float turn_speed = 0.2; 
    float correction;
    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);
    thinggy.printf("Pimpin");
    
    wait(1.0);
 
    thinggy.sensor_auto_calibrate();
 
    thinggy.calibrated_sensor(sensor);
    
    //find the average of the three sensors 
    returned = (sensor[1] + sensor[2] + sensor[3])/3;
 
    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);
                    thinggy.right_motor(-turn_speed);
                    thinggy.calibrated_sensor(sensor);
                }
                //turns left 
                while(sensor[4] > sensor[0] && thinggy.line_position() != 0){
                    thinggy.left_motor(-turn_speed);
                    thinggy.right_motor(turn_speed);
                    thinggy.calibrated_sensor(sensor);
                }
                thinggy.calibrated_sensor(sensor);
                returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
        }//while returned <= 220
        
        // Curves and straightaways    
        while(returned > 240){
            
            cross_detection(sensor, black_thresh, white_thresh); 
        
            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);
                thinggy.left_motor(speed-correction);
            }    
            
            //speed limiting for left motor
            if(speed - correction > 1){
                thinggy.left_motor(0.6);
                thinggy.right_motor(speed+correction);
            }
            else{
                thinggy.left_motor(speed-correction);
                thinggy.right_motor(speed+correction);
                
               //Infared: stop if obstructed           
                m3pi_IN[0].mode(PullUp);
                while (m3pi_IN[0]==0){
                    thinggy.stop();
                    }
            }
            thinggy.calibrated_sensor(sensor);
            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); 
        } 
}