This ILC code kinda works
Dependencies: FatFileSystem MSCFileSystem btbee m3pi_ng mbed ILCRobot
Fork of RobotB by
main.cpp
- Committer:
- charwhit
- Date:
- 2015-06-11
- Revision:
- 19:4173c41c263b
- Parent:
- 18:dcdf28fa4c9e
- Child:
- 20:262739736935
File content as of revision 19:4173c41c263b:
#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 = 27; int last_lap = 0; int position = 0; float llpp = 0; //last lap previous point float llnp = 0; //last lap next point float llcp = 0; //last lap current point 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]; int prevpos, nextpos; float traveled = 0; 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, Currentvec; 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) { timer.start(); lap= lap +1; } else { 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; robot.cls(); robot.print("%f", lap_time); 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; if(last_lap != lap){ position = 0; llpp = llcp = llnp = 0; } last_lap = lap; // compute the power if(lap == 1){ power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM)); Uvalue.push_back(power); Currentvec.push_back(current_pos); } else if(lap == 0){ power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM)); } else{ if (position < Currentvec.size()){ D_TERM = 13.5; if (position != 0) prevpos = Currentvec[position-1]; else prevpos = Currentvec.back(); if (position != Currentvec.size() - 1) nextpos = Currentvec[position+1]; else nextpos = Currentvec.front(); power = Uvalue[position] + (P_TERM)* current_pos + (D_TERM*(prevpos + nextpos)); if (btbee.writeable()) btbee.printf("Power: %f\n", power); Uvalue[position] = power; Currentvec[position] = current_pos; position++; } else 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); //traveled += ((right + left) / 2) * .05; // Uvalue.push_back(power); // Currentvec.push_back(current_pos); // 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);*/ }