File content as of revision 22:1288bd38d682:

#include "mbed.h"
#include "MSCFileSystem.h"
#include "btbee.h"
#include "m3pi_ng.h"
#include <fstream>
#define FSNAME "msc"
#include <string>
#include <sstream>
#include <vector>
#include "math.h"
#include <algorithm>


MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"

m3pi robot;
btbee btbee;
DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
DigitalIn m3pi_pb(p21);
Timer timer;
Timer time_wait;
#define MAX .96
#define MIN 0
#define PI 3.14159265

//#define P_TERM 5
//#define I_TERM 0
//#define D_TERM 20


//Struct that represents test points in Nedler-Mead. Of the form (P,D) i.e (P=5,D=7)
struct testPoint {
    testPoint():Pvalue(0), Dvalue(0), time(1000) {}
    testPoint(float a, float b, float c): Pvalue(a), Dvalue(b), time(c) {}

    friend testPoint operator-(const testPoint& lhs, const testPoint& rhs) {
        testPoint dummy;
        dummy.Pvalue = lhs.Pvalue - rhs.Pvalue;
        dummy.Dvalue = lhs.Dvalue - rhs.Dvalue;
        dummy.time = 1000;
        return dummy;
    }

    friend testPoint operator+(const testPoint& lhs, const testPoint& rhs) {
        testPoint dummy;
        dummy.Pvalue = lhs.Pvalue + rhs.Pvalue;
        dummy.Dvalue = lhs.Dvalue + rhs.Dvalue;
        dummy.time = 1000;
        return dummy;
    }

    friend testPoint operator*(const float a, const testPoint& rhs) {
        testPoint dummy;
        dummy.Pvalue = a * rhs.Pvalue;
        dummy.Dvalue = a * rhs.Dvalue;
        dummy.time = 1000;
        return dummy;

    }



    float Pvalue;
    float Dvalue;
    float time;
};


//Comparator for testPoint struct sorting
bool myComp (testPoint i,testPoint j)
{
    return (i.time<j.time);
}




int main()
{
    float P_TERM = 3;
    float I_TERM = 0;
    float D_TERM = 27;
    

    m3pi_pb.mode(PullUp);
    btbee.reset();


    robot.sensor_auto_calibrate();
    //Waiting for button to be pressed before starting for bluetooth comms
    while(m3pi_pb) {

    }
    wait(2.0);
    float right;
    float left;
    float current_pos = 0.0;
    float previous_pos =0.0;
    float derivative, proportional, integral = 0;
    float power;
    float speed = MAX;

    int lap = 0;
    float lap_time = 0.0;
    int y =1;

    bool passed = false;

    Timer crossProtect;


    //Simplex algorithm stuff
    vector< testPoint > triangle;
    triangle.push_back(testPoint(20,2,1000));
    triangle.push_back(testPoint(2,40,1000));
    triangle.push_back(testPoint(3,27,1000));
    int corners = 0;
    P_TERM = triangle[0].Pvalue;
    D_TERM = triangle[0].Dvalue;
    testPoint centroid;
    testPoint reflection;
    testPoint expansion;
    testPoint contraction;
    testPoint reduction;
    int step = 1;
    bool contracted = false;
    float reflecFactor = 1;
    float expanFactor = 2;
    float contrFactor = -1/2;
    float reducFactor = 1/2;
    Timer backup; //Timer so robot doesnt count a lap when it swerves wierdly
    
    //Max and min values for P and D parameters
    int MINPVAL = 2;
    int MAXPVAL = 30;
    int MAXDVAL= 40;



    time_wait.start();
    backup.start();
    int x [5];
    while(y) {
        time_wait.reset();
        
        //Get raw sensor values
        robot.calibrated_sensor(x);

        //Check to make sure battery isn't low
        if (robot.battery() < 2.4) {
            timer.stop();
            robot.printf("LowBatt");
            btbee.printf("Battery too low\n");
            break;
        }

        

        //running the four corners
        if(x[0] > 300 && x[2]>300 && x[4]>300 && !passed && (backup.read() > 2 || lap == 0)) {
            btbee.printf("Crossing\n");
            if (lap == 0) { //Allows for 10cm head start
                timer.start();
                lap= lap +1;
            } else if(corners < 2) { // This condition get lap times for the intial group of 3 points
                lap_time = timer.read();
                triangle[corners].time = lap_time;
                
                btbee.printf("Lap %d time: %f\n", lap, lap_time);
                

                ++corners;
                robot.cls();
                robot.printf("%f", lap_time);

                ++lap;
                P_TERM = triangle[corners].Pvalue;
                D_TERM = triangle[corners].Dvalue;
                timer.reset();
            } else {
                if (corners == 2){
                    triangle[corners].time = timer.read();
                    ++corners;
                }
                switch(step) { //Each step corresponds to a step in Nedler-Mead
                    case 1:
                        if (triangle[1].time == 1000 || triangle[2].time == 1000){ //Getting new data if reduced
                            if (triangle[1].time == 1000){
                                triangle[1].time = timer.read();
                                P_TERM = triangle[2].Pvalue;
                                D_TERM = triangle[2].Dvalue;
                                timer.reset();
                                break;
                            }
                            else
                                triangle[2].time = timer.read();
                        }
                        sort(triangle.begin(), triangle.end(), myComp);
                        for (int i = 0; i < triangle.size(); ++i)
                            btbee.printf("Time %d: %f\n",i,triangle[i].time);
                        btbee.printf("Best so far: P: %f, D: %f\n", triangle[0].Pvalue, triangle[0].Dvalue);
                        centroid.Pvalue = (triangle[0].Pvalue + triangle[1].Pvalue) / 2;
                        centroid.Dvalue = (triangle[0].Dvalue + triangle[1].Dvalue) / 2;
                        reflection = centroid + (reflecFactor*(centroid - triangle[2]));
                        if (reflection.Pvalue < 0)
                            reflection.Pvalue = MINPVAL;
                        else if (reflection.Pvalue > 30)
                            reflection.Pvalue = MAXPVAL;

                        if (reflection.Dvalue < 0)
                            reflection.Dvalue = 1;
                        else if (reflection.Dvalue > 40)
                            reflection.Dvalue = MAXDVAL;
                        P_TERM = reflection.Pvalue;
                        D_TERM = reflection.Dvalue;
                        ++step;
                        btbee.printf("Reflecting\n");
                        btbee.printf("P: %f, D: %f\n", P_TERM, D_TERM);
                        poop = true;
                        timer.reset();
                        break;
                    case 2: //finished running reflection
                        reflection.time = timer.read();
                        btbee.printf("Reflection Time: %f\n", reflection.time);
                        if (reflection.time < triangle[0].time) {
                            expansion = centroid + expanFactor*(centroid - triangle[2]);
                            if (expansion.Pvalue < 0)
                                expansion.Pvalue = MINPVAL;
                            else if (expansion.Pvalue > 30)
                                expansion.Pvalue = MAXPVAL;

                            if (expansion.Dvalue < 0)
                                expansion.Dvalue = 1;
                            else if (expansion.Dvalue > 40)
                                expansion.Dvalue = MAXDVAL;
                            P_TERM = expansion.Pvalue;
                            D_TERM = expansion.Dvalue;
                            ++step;
                            btbee.printf("Expanding\n");
                            btbee.printf("P: %f, D: %f\n", P_TERM, D_TERM);
                            timer.reset();
                        } else if (reflection.time < triangle[1].time) {
                            triangle[2] = reflection;
                            step = 1;
                            btbee.printf("Going back to 1\n");
                            P_TERM = 3;
                            D_TERM = 27;
                        } else {

                            contraction = centroid + contrFactor*(centroid - triangle[2]);
                            if (contraction.Pvalue < 0)
                                contraction.Pvalue = MINPVAL;
                            else if (contraction.Pvalue > 30)
                                contraction.Pvalue = MAXPVAL;

                            if (contraction.Dvalue < 0)
                                contraction.Dvalue = 1;
                            else if (contraction.Dvalue > 40)
                                contraction.Dvalue = MAXDVAL;
                            P_TERM = contraction.Pvalue;
                            D_TERM = contraction.Dvalue;
                            ++step;
                            contracted = true;
                            btbee.printf("Contracting\n");
                            btbee.printf("P: %f, D: %f\n", P_TERM, D_TERM);
                            timer.reset();
                        }
                        break;
                    case 3: //Finished running Expansion/Contraction
                        if (!contracted) {
                            expansion.time = timer.read();
                            btbee.printf("Expansion Time: %f\n", expansion.time);
                            if (expansion.time < reflection.time)
                                triangle[2] = expansion;
                            else
                                triangle[2] = reflection;
                            P_TERM = 3;
                            D_TERM = 27;
                        } else {
                            contraction.time = timer.read();
                            btbee.printf("Contraction Time: %f\n", contraction.time);
                            if (contraction.time < triangle[2].time){
                                triangle[2] = contraction;
                                P_TERM = 3;
                                D_TERM = 27;
                            }
                            else {
                                triangle[1] = triangle[0] + (reducFactor*(triangle[1] - triangle[0]));
                                triangle[2] = triangle[0] + (reducFactor*(triangle[2] - triangle[0]));
                                P_TERM = triangle[1].Pvalue;
                                D_TERM = triangle[1].Dvalue;
                                timer.reset();
                            }
                        }
                        step = 1;
                        contracted = false;
                        btbee.printf("Going back to 1\n");
                        break;
                }//end of switch
                btbee.printf("Step: %d\n", step);
            }
            passed = true;
            backup.reset();
        } else if (x[0] > 300 && x[2]>300 && x[4]>300)
            passed = true;
        else
            passed = false;



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


}