File content as of revision 16:caa77287cc25:

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


MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
//Serial pc(USBTX,USBRX);

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

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



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

    btbee.reset();
    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;

    int lap = 0;
    float lap_time = 0.0;
    float total_time = 0.0;
    float average_time = 0.0;
    int y =1;
    int count = 0;
    float paramChange[3];
    bool passed = false;
    char sweepValue[100];



    char arr_read[30]; // this should be long enough to store any reply coming in over bt.
    int  chars_read;
    vector<float> Uvalue, lineposval, setPointVals;
    vector<float> sweepData;
    FILE *fp = fopen( "/" FSNAME "/exper1.txt", "w");
    //FILE *sweep = fopen("/" FSNAME "/sweep15.txt","r");



    /*if (sweep == NULL) {
        robot.printf("Nope.");
        y = 0;
    }*/


    /* for (int i = 0; i <5; ++i)
         current_pos[i] = 0.0; */


    //wait(8);
    btbee.printf("Battery: %f\n", robot.battery());


    //btbee.printf("C: %d", sweepData.size());


    time_wait.start();
    int x [5];
    //timer.start();
    while(y) {
        time_wait.reset();
        //timer.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;
        }

        //else if (m3pi_IN [0] == 0)
        //{break;}

        if( (x[0] > 300 && x[2]>300 && x[4]>300 & !passed)) {
            if (lap == 0) {
                /*while( x[0]> 300 && x[4] > 300) {
                    robot.calibrated_sensor(x);
                }*/
                timer.start();
                lap= lap +1;
            }

            else if (lap == 1) {

                robot.stop();
                
                //Print to file stuff
                /*if(fp != NULL) {
                    for (int i = 0; i < Uvalue.size(); ++i)
                        fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]);
                    fclose(fp);
                    //fclose(sweep);
                    robot.cls();
                    robot.printf("Doner");
                }
                break;*/
                lap_time = timer.read();
                total_time += lap_time;
                average_time = total_time/lap;
                robot.printf("%f",average_time);
                if (btbee.writeable()) {
                    btbee.printf("Lap %d time: %f\n", lap, lap_time);
                    btbee.printf("Avg Lap time: %f\n", average_time);
                }

                /*while (count < 3) {
                    btbee.read_line(arr_read, 30, &chars_read);
                    paramChange[count] = atof(arr_read);
                    count++;
                }
                P_TERM = paramChange[0];
                I_TERM = paramChange[1];
                D_TERM = paramChange[2];
                btbee.printf("PTERM %f\n", P_TERM);
                btbee.printf("ITERM %f\n", I_TERM);
                btbee.printf("DTERM %f\n", D_TERM);
                lap = 0;
                total_time = 0;
                count = 0;
                timer.stop();
                timer.reset();
                continue;*/
                break;

            } else {
                /*while( x[0]> 300 && x[4] > 300) {
                    robot.calibrated_sensor(x);
                }*/
                lap_time = timer.read();
                if (btbee.writeable()) {
                    btbee.printf("Lap %d time: %f\n", lap, lap_time);
                }
                total_time += lap_time;
                average_time = total_time/lap;
                lap = lap +1;
                timer.reset();
            }
            passed = true;
        } else if (x[0] > 300 && x[2]>300 && x[4]>300)
            passed = true;
        else
            passed = false;



        // Get the position of the line.
        /*  for (int i =0; i < 4; ++i)
             current_pos[i] = current_pos[i+1];
           current_pos[4] = robot.line_position();
        proportional = current_pos[4];

        // compute the derivative
        derivative = 0;
        for (int i =1; i<5;++i) {
             if (i ==1)
                 derivative += 0*(current_pos[i] - current_pos[i-1]);
              else if (i == 2)
                 derivative += 0*(current_pos[i] - current_pos[i-1]);
              else if (i==3)
                 derivative += 0*(current_pos[i] - current_pos[i-1]);
             else
                 derivative += (current_pos[i] - current_pos[i-1]);
         }

        derivative = derivative; */


        current_pos = robot.line_position();

        //Performs the sweep
        /*if (fgets(sweepValue, 100, sweep) != NULL){
            proportional =  atof(sweepValue) + current_pos;
        }
        else {
            robot.stop();
            //fclose(sweep);
            if(fp != NULL) {
                for (int i = 0; i < Uvalue.size(); ++i)
                    fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]);
                fclose(fp);
                fclose(sweep);
                robot.cls();
                robot.printf("Doner");
            }
            break;
            //FILE *sweep = fopen("/" FSNAME "/sweep15.txt","r");
            //robot.cls();
            //robot.printf("Done.");
            // break;
        }*/

        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));
        /*if (fgets(sweepValue, 100, sweep) != NULL){
            power =  atof(sweepValue) + power;
        }
        else {
            robot.stop();
            fclose(sweep);
            if(fp != NULL) {
                for (int i = 0; i < Uvalue.size(); ++i)
                    fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]);
                fclose(fp);
                fclose(sweep);
                robot.cls();
                robot.printf("Doner");
            }
            break;
        }*/
        //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);



        Uvalue.push_back(power);
        lineposval.push_back(robot.line_position());
        setPointVals.push_back(0);
        //setPointVals.push_back(0.5);



        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);*/




}