This ILC code kinda works
Dependencies: FatFileSystem MSCFileSystem btbee m3pi_ng mbed ILCRobot
Fork of RobotB by
Diff: main.cpp
- Revision:
- 14:5b3cc7741bdf
- Parent:
- 13:9bb4ac4968d2
- Child:
- 15:3e3f1b42cc23
diff -r 9bb4ac4968d2 -r 5b3cc7741bdf main.cpp --- a/main.cpp Mon Jun 01 08:10:10 2015 +0000 +++ b/main.cpp Mon Jun 01 09:33:43 2015 +0000 @@ -18,7 +18,7 @@ DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf Timer timer; Timer time_wait; -#define MAX .15 +#define MAX .35 #define MIN 0 #define PI 3.14159265 @@ -32,7 +32,7 @@ { float P_TERM = 1; float I_TERM = 0; - float D_TERM = 2; + float D_TERM = 0; btbee.reset(); robot.sensor_auto_calibrate(); @@ -55,22 +55,19 @@ float paramChange[3]; bool passed = false; char sweepValue[100]; - int import = 0; + //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; + 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 - float t = 0; - float A = 0.1; - float f = 19.8/20; - float newSetPoint; + if (sweep == NULL){ @@ -112,7 +109,7 @@ //else if (m3pi_IN [0] == 0) //{break;} - if( (x[0] > 300 && x[2]>300 && x[4]>300 & !passed) || timer.read() > 5) { + 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); @@ -121,12 +118,12 @@ lap= lap +1; } - else if (lap == 1 || timer.read() > 5) { + else if (lap == 1) { robot.stop(); robot.printf("Size: %d", Uvalue.size()); if(fp != NULL){ for (int i = 0; i < Uvalue.size(); ++i) - fprintf(fp,"%f %f\n",Uvalue[i], lineposval[i]); + fprintf(fp,"%f %f %f\n", setPointVals[i], Uvalue[i], lineposval[i]); fclose(fp); fclose(sweep); robot.cls(); @@ -208,15 +205,13 @@ current_pos = robot.line_position(); if (fgets(sweepValue, 100, sweep) != NULL){ - - //f = 19.8/20 * t + .2; - //newSetPoint = A*sin(2*PI*f*t); + proportional = atof(sweepValue) + current_pos; } else{ - robot.printf("Try Again"); - break; + fclose(sweep); + FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r"); } @@ -257,10 +252,11 @@ //btbee.printf("%f %f %f\n", left, right, robot.line_position()); Uvalue.push_back(power); lineposval.push_back(robot.line_position()); + setPointVals.push_back(atof(sweepValue)); //} - //++import; - t = t + .005; + + wait((5-time_wait.read_ms())/1000); }