File content as of revision 10:e2267771f7d4:

#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;

//EFFECTS: looks at a number of sensors to determine if the robot has reached a cross. 
 //         If the robot has reached a cross, it returns TRUE. If not, it returns FALSE
bool cross_detection(int sensor[5], int black_thresh, int white_thresh); 

// REQUIRES: Startpoint and endpoint must be between 1 and 6, inclusive
// EFFECTS: returns a string of directions fo either L (left), R (right), or
//          F (forward) to tell the robot how to get from Startpoint to endpoints 
string directions(int startpoint, int endpoint);


void mapping(string directions, float speed, int turns);

void turn_right();
void turn_left();
 
m3pi thinggy; 

//black and white thresholds chose after testing
int black_thresh = 300;
int white_thresh = 240;

//normal speed and turn speed to slow 
float speed = 0.25;
float turn_speed = 0.2; 

//used to change the direction of the car
float correction;

//k was chosen after testing 
float k = -0.3;

int sensor[5];
int returned;  

 
int main() { 
    bool cross = 0; 
    
    wait(1.0);
    
    //calibrate the sensors 
    thinggy.sensor_auto_calibrate();
    thinggy.calibrated_sensor(sensor);
    
    //find the average of the three sensors 
    returned = (sensor[1] + sensor[2] + sensor[3])/3;
    
    //enter in the endpoint and startpoint here 
    int startpt = 2; 
    int endpt = 1; 
    string d = directions(startpt, endpt);
    
    //declares the number of turns so that it can be incremented after each turn
    int turns = 0; 
 
    //performs movement
    while(1) {
        
        //checks 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);
                    
                    //checks if there is a cross
                    if(cross){
                        mapping(d, speed, turns);
                        ++turns;
                        cross = 0;  
                    }
                    //corrects turn
                    else {
                        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);
                    //checks for cross
                    if(cross){
                        mapping(d, speed, turns);
                        ++turns; 
                        cross = 0; 
                    }
                    //corrects pathway
                    else{
                        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);
            //checks for cross
            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: will stop if obstructed           
                m3pi_IN[0].mode(PullUp);
                while (m3pi_IN[0]==0){
                    thinggy.stop();
                }
            }
            
            //recalibrate 
            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)
    

}

 //EFFECTS: looks at a number of sensors to determine if the robot has reached a cross. 
 //         If the robot has reached a cross, it returns TRUE. If not, it returns FALSE
 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; 
}


// REQUIRES: Startpoint and endpoint must be between 1 and 6, inclusive
// EFFECTS: returns a string of directions fo either L (left), R (right), or
//          F (forward) to tell the robot how to get from Startpoint to endpoints 
string directions(int startpoint, int endpoint){
    string charmap[6][6] = {
       {"", "RLRLLR", "RLRLFF", "RLRRRLLF", "RLRRLFLRRRL", "RLRRLR"}, 
       {"LRRLR", "", "LLF", "LLRLL", "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];
}

//MODIFIES: Direction and speed 
//EFFECTS: Looks at the turn in the string of directions and tells it whether 
//          whether to go left, right, forward 
void mapping(string directions, float speed, int turns){
    
    //char x is L, R, F, or NULL to perform at the specified turn 
    char x = directions[turns]; 
    
    //left turn
    if(x == 'L'){
        turn_left();
        return; 
    }
    //right turn 
    else if(x == 'R'){
        turn_right();
        return; 
    }
    //move forward 
    else if(x == 'F'){
        thinggy.forward(speed); 
        wait(.1); 
        return; 
    }
    //if it reaches the last element in the string 
    else if (x == NULL){
        thinggy.stop(); 
        wait(300); 
        return; 
    }
}

//EFFECTS: turns the robot left
void turn_left(){
  int n=0;
  wait(.08); 
    while (n<700){
        thinggy.left_motor(-0.2);
        thinggy.right_motor(0.2);
        n++;
        }
    thinggy.forward(0.2);
    wait(0.15);          
}

//EFFECTS: turns the robot right 
void turn_right(){
    int n=0;
    wait(.08); 
    while (n<700){
        thinggy.left_motor(0.2);
        thinggy.right_motor(-0.2);
        n++;
        }
    thinggy.forward(0.2);
    wait(0.15);      
}