File content as of revision 3:4284d7cdb68e:

#include "mbed.h"
#include "btbee.h"
#include "m3pi_ng.h"
#include <vector>
#include <string>
m3pi robot;
Timer timer;
Timer time_wait;
#define MAX 0.5
#define MIN 0

#define P_TERM 2.5
#define I_TERM 2
#define D_TERM 23

void turnRight();
void turnLeft();
void goStraight();

int main()
{

    robot.sensor_auto_calibrate();
    wait(2.0);

    float right;
    float left;
    //float current_pos[5];
    float current_pos = 0.0;
    float previous_pos =0.0;
    float derivative, proportional, integral = 0;
    float power;
    float speed = MAX;
    string turns;
    int direction = 0;
    int x [5];
    bool passed = false;


    
    int Run = 1;
    string blah[36] = {"", "LRRL", "SSRL", "RLSRL", "RLLLRSRLLR", "LRLL",
                    "RLLR", "", "SSR", "RLRR", "RLLRLSR", "LRSLR",
                    "RLSS", "LLS", "", "RR", "RLSLR", "LRSSS",
                    "RLSRL", "LLRL", "LL", "", "RLSS", "RLRLS",
                    "RRSRR", "LSRLRRL", "LRSRL", "SSRL", "", "RLRRRL",
                    "RRR", "LRSLR", "SSSLR", "SRLL", "RLLLRL", ""};
    //vector< vector<string> > Lookup(6, vector<string>(6));
    
    //Lookup[1][2] = "LLS";
    

    


    time_wait.start();
    

    while(Run) {
        time_wait.reset();
        //Get raw sensor values
        
        robot.calibrated_sensor(x);

        //insert end and beginning locations

        turns = blah[(end-1)*6+(start-1)];

        //Check to make sure battery isn't low
        if (robot.battery() < 2.4) {
            timer.stop();
            break;
        }

        
        
        if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
            
            
            if(turns.at(direction) == 'L'){
                while(x[0] > 300){
                    goStraight();
                    robot.calibrated_sensor(x);
                    }
                previous_pos = 0;
                robot.stop();
                //wait(1);
                 while(x[2] > 300){
                    turnLeft();
                    robot.calibrated_sensor(x);
                }
               
                while((x[0] > 300) || (x[2] < 300)){
                    turnLeft();
                    robot.calibrated_sensor(x);
                    }
                robot.stop();
                //continue;
                /*wait(1);
                goStraight();
                wait(1);*/
            }
            else if(turns.at(direction) == 'R'){
                previous_pos = 0;
                robot.stop();
                while(x[4] > 300){
                    goStraight();
                    robot.calibrated_sensor(x);
                }
                //wait(1);
                while(x[2] > 300){
                    turnRight();
                    robot.calibrated_sensor(x);
                }
                
                while((x[4] > 300) || (x[2] < 300)){
                    turnRight();
                    robot.calibrated_sensor(x);
                    }
                robot.stop();
                
                /*wait(1);
                goStraight();
                wait(1);*/
            }
            else{
                robot.stop();
                //wait(1);
                while(x[0] > 300 || x[4] > 300){
                    goStraight();
                    robot.calibrated_sensor(x);
                }
                //wait(1);
            }
            robot.cls();
            robot.printf("Size %d", direction);
            ++direction;
            passed = true;
            continue;
        }
        else if (x[0]>300 || x[4]>300 && direction >= turns.size()){
            robot.stop();
            robot.cls();
            robot.printf("Try again");
            break;
        }
        else if (x[0]>300 || x[4]>300 && passed)
            passed = true;
        else
            passed = false;


        // Get the position of the line.
        current_pos = robot.line_position();
        proportional = current_pos;

        derivative = current_pos - previous_pos;


        //compute the integral
        integral =+ proportional;

        //remember the last position.
        previous_pos = current_pos;

        // compute the power
        power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
        //computer new speeds
        right = speed+power;
        left = speed-power;

        //limit checks
        if(right<MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;

        if(left<MIN)
            left = MIN;
        else if (left>MIN)
            left = MAX;

        //set speed

        robot.left_motor(left);
        robot.right_motor(right);

        wait((5-time_wait.read_ms())/1000);
    }



    robot.stop();

    /*char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
                  ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
                  ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
                  ,'G','8','E','8','D','8','C','4'
                 };
    int numb = 59;

    robot.playtune(hail,numb);*/


}


void turnRight(){
    //Timer turner;
    //turner.start();
    //while (turner.read_ms() < 10){
            robot.right(.5);
    //}
    //turner.stop();
}

void turnLeft(){
//    Timer turner;
  //  turner.start();
  //  while (turner.read_ms() < 10){
            robot.left(.5);
   // }
    //turner.stop();
}

void goStraight(){
   // Timer turner;
   // turner.start();
   // while (turner.read_ms() < 5){
            robot.forward(.5);
   // }
   // turner.stop();
}