This ILC code kinda works
Dependencies: FatFileSystem MSCFileSystem btbee m3pi_ng mbed ILCRobot
Fork of RobotB by
Diff: main.cpp
- Revision:
- 8:bd2f012e2f57
- Parent:
- 7:7d491b51665e
- Child:
- 9:030b7e4ff7be
--- a/main.cpp Wed May 27 13:36:42 2015 +0000 +++ b/main.cpp Thu May 28 13:40:16 2015 +0000 @@ -1,12 +1,20 @@ #include "mbed.h" +#include "MSCFileSystem.h" #include "btbee.h" #include "m3pi_ng.h" +#include <fstream> +#define FSNAME "msc" + + +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 0.95 +#define MAX 1 #define MIN 0 //#define P_TERM 5 @@ -17,8 +25,8 @@ int main() { - float P_TERM = 1; - float I_TERM = 0; + float P_TERM = 2.5; + float I_TERM = .5; float D_TERM = 20; btbee.reset(); @@ -40,9 +48,18 @@ int y =1; int count = 0; float paramChange[3]; + bool passed = false; char arr_read[30]; // this should be long enough to store any reply coming in over bt. int chars_read; + + int check = msc.disk_initialize(); + + robot.locate(0,0); + robot.printf("USBWrite"); + robot.locate(0,1); + robot.printf("Test"); + ofstream myFile ("/" FSNAME "/data.txt"); /* for (int i = 0; i <5; ++i) current_pos[i] = 0.0; */ @@ -72,16 +89,16 @@ //else if (m3pi_IN [0] == 0) //{break;} - if( x[0] > 300 && x[2]>300 && x[4]>300) { + if( x[0] > 300 && x[2]>300 && x[4]>300 & !passed) { if (lap == 0) { - while( x[0]> 300 && x[4] > 300) { + /*while( x[0]> 300 && x[4] > 300) { robot.calibrated_sensor(x); - } + }*/ timer.start(); lap= lap +1; } - else if (lap == 2) { + else if (lap == 5) { robot.stop(); lap_time = timer.read(); total_time += lap_time; @@ -113,9 +130,9 @@ continue; } else { - while( x[0]> 300 && x[4] > 300) { + /*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); @@ -125,7 +142,13 @@ 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. @@ -183,6 +206,19 @@ robot.left_motor(left); robot.right_motor(right); + + + if (myFile.is_open()) + { + myFile << left << " " << right << " " << robot.line_position(); + robot.cls(); + robot.locate(0,0); + robot.printf("Done."); + } + else + { + robot.printf("Error."); + } wait((5-time_wait.read_ms())/1000); }