File content as of revision 6:a6f752174eef:

#include "mbed.h"
#include "m3pi_ng.h"
#include "cmath"
#include "iostream"
#include <string>

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

using namespace std;

bool cross_detection(int sensor[5], int black_thresh, int white_thresh); 
void mapping(string directions, float speed, int turns);
string directions(int startpoint, int endpoint);
int end(); 
int start(); 

 
m3pi thinggy; 

    int black_thresh = 300;
    int white_thresh = 240;
    float speed = 0.25;
    float turn_speed = 0.2; 
    float correction;
    float k = -0.3;
    int sensor[5];
    int returned;  

 
int main() { 
    bool cross = 0; 
    
    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;
    
    //finds the directions of the robot 
    int startpt = 2; //start();  
    int endpt = 3; //end(); 
    string d = directions(startpt, endpt);
    int turns = 0; 
 
    while(1) {
        //check if it needs to turn 
        while(returned <= 240){
                //turns right 
                while(sensor[0] < sensor[4] && thinggy.line_position() != 0){
                    cross = cross_detection(sensor, black_thresh, white_thresh);
                    if(cross){
                        mapping(d, speed, turns);
                        ++turns;
                        cross = 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){
                    cross = cross_detection(sensor, black_thresh, white_thresh);
                    if(cross){
                        mapping(d, speed, turns);
                        ++turns; 
                        cross = 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){
            float position = thinggy.line_position();
            correction = k*(position);
            cross = cross_detection(sensor, black_thresh, white_thresh);
            if(cross){
                mapping(d, speed, turns);
                ++turns;
                cross = 0;  
            }
            // -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
            else 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)
    

}

//DONE 
 //REQUIRES: array of 5 ints 
 //EFFECTS: stops the robot if it comes to any interesection where a decision has to be made
 // and returns true if there is a cross
 bool 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[2] > black_thresh and sensor[4] > black_thresh){
            return 1;  
        }
        //left or forward 
        else if(sensor[0] > black_thresh and sensor[2] > black_thresh){
            return 1; 
        }
        //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 ){
            return 1;  
        } 
        //forward or right
        else if (sensor[2] > black_thresh and sensor[4] > black_thresh){
            return 1; 
        } 
        
        return 0; 
}

//take in the starting point of the robot from bluetooth 
int start(){
 return 0;  
}

//take in the ending point of the robot from bluetooth 
int end(){
  return 0; 
}

//DONE
//gives the string to use for the map 
string directions(int startpoint, int endpoint){
    string charmap[6][6] = {
       {"", "RLLR", "RLFF", "RRRLLF", "RRLFLRRRL", "RRLR"}, 
       {"LRRL", "", "LLF", "LFRLLF", "LFRLRRL", "LRFLR"},
       {"FFRL", "FRR", "", "LL", "FLRLRRL", "FFFLR"},
       {"FRRLRL", "RLRR", "RR", "", "FFRL", "FRLRL"},
       {"RLLLRFRLL", "RLLRLFR", "RLFLR", "RLFF", "", "RLLLRL"},
       {"LRLL", "LFLR", "LRFFF", "RLRLF", "RLRRRL", ""}
    }; 
    return charmap[startpoint - 1][endpoint - 1];
}

//DONE
//takes in the string of directions and the number of turns 
//completed and then tells it whether to do left, right, forward 
void mapping(string directions, float speed, int turns){
    
    char x = directions[turns]; //something in the string 
    
    //tells it which direction to go 
    if(x == 'L'){
        thinggy.locate(0,1);
        thinggy.printf("Left");
        thinggy.locate(0,0);
        thinggy.printf("Pimpin");
        thinggy.calibrated_sensor(sensor);
        returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
        while(returned <= 240){
                //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
        return; 
    }
    else if(x == 'R'){
        thinggy.calibrated_sensor(sensor);
        returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
        while(returned <= 240){
                //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);
                }
                thinggy.calibrated_sensor(sensor);
                returned = (sensor[1] + sensor[2]*2 + sensor[3])/4;
                
        }//while returned <= 220
        return; 
    }
    else if(x == 'F'){
        //do nothing 
        return; 
    }
    else if (x == NULL){
        thinggy.stop(); 
        wait(300); 
        return; 
    }
}