File content as of revision 10:d1795905c412:

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

//Access infared sensors
DigitalIn m3pi_IN[] = {(p12)};
DigitalIn m3pi_pb = (p21);
DigitalOut led_1=(p13);
DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
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);
void turn_right();
void turn_left();

int end(); 
int start(); 

 
m3pi thinggy; 
btbee btbee;

    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 
    char startpt; //start();  
    char endpt; //end(); 
    char arr_read[2] = {'0','0'};
    int chars_read;
    btbee.reset();
    
   while(m3pi_pb) {
       m3pi_led[0]=!m3pi_led[0];
       wait(0.1);
       btbee.printf("Go!\n");
   }
    // check for answers after each write /not write
   
           btbee.read_all(arr_read, 2, &chars_read );
           thinggy.locate(0,0);
           thinggy.cls();
          
            startpt = arr_read[0];
            endpt = arr_read[1]; 
  
    wait(0.1);
    thinggy.printf("Going");
    thinggy.locate(0,1); 
    thinggy.printf("%c", startpt);
    thinggy.printf(" ");
    thinggy.printf("%c", endpt);
    
    //turn char endpoints into int endpoints
    int startpt_int = startpt - '0';
    int endpt_int   = endpt   - '0';
    
    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;  
                    }
                    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);
                    if(cross){
                        mapping(d, speed, turns);
                        ++turns; 
                        cross = 0; 
                    }
                    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);
            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] = {
       {"", "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];
}

//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'){
        turn_left();
        return; 
    }
    else if(x == 'R'){
        turn_right();
        return; 
    }
    else if(x == 'F'){
        thinggy.printf("F"); 
        thinggy.forward(speed); 
        wait(.1); 
        return; 
    }
    else if (x == NULL){
        thinggy.stop(); 
        wait(300); 
        return; 
    }
}

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);          
    
}

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);      
    
}