File content as of revision 16:3e3f1b42cc23:

#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 .95
#define MIN 0
#define PI 3.14159265

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



int main()
{
    float P_TERM = 2.5;
    float I_TERM = .5;
    float D_TERM = 25;

    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];
  
    //string hallo = "Idk what is going on...\n";
    //stringstream ss;

    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 "/data.txt", "w");
    //FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
    
    //Manual implementation of sweep function
   
    
    
    /*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());
    //timer.start();
    
    /*while (fgets(sweepValue, 100, sweep) != NULL){
        sweepData.push_back(atof(sweepValue));
    }*/
    btbee.printf("C: %d", sweepData.size());


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



        //Check to make sure battery isn't low
        if (robot.battery() < 2.4) {
            timer.stop();
            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.printf("Success.");
                robot.stop();
                robot.printf("Size: %d", Uvalue.size());
                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.locate(0,0);
                    robot.printf("Doner");
                }
                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.printf("Input parameter\n");
                    btbee.read_line(arr_read, 30, &chars_read);
                    paramChange[count] = atof(arr_read);
                    //btbee.printf("%d", 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();
                y = 0;
                continue;
                
            } 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();
       //if (fgets(sweepValue, 100, sweep) != NULL){
    
               // proportional =  atof(sweepValue) + current_pos;
               proportional =  current_pos;
                
       /* }
        else{
            fclose(sweep);
            FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
        }*/
        

        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);
        
        
        //if (myFile.is_open())
        //{
            //btbee.printf("%f %f %f\n", left, right, robot.line_position());
            Uvalue.push_back(power);
            lineposval.push_back(robot.line_position());
            setPointVals.push_back(0);
        //}
    
        
       
        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);




}