Diff: main.cpp

Revision:

13:0422156f83f6

Parent:

12:bb90691e5166

Child:

14:9bb4ac4968d2

--- a/main.cpp	Fri May 29 13:37:39 2015 +0000
+++ b/main.cpp	Mon Jun 01 08:06:02 2015 +0000
@@ -1,4 +1,4 @@
-#include =-pokm rtfg"mbed.h"
+#include "mbed.h"
 #include "MSCFileSystem.h"
 #include "btbee.h"
 #include "m3pi_ng.h"
@@ -7,6 +7,7 @@
 #include <string>
 #include <sstream>
 #include <vector>
+#include "math.h"
 
 
 MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
@@ -19,6 +20,7 @@
 Timer time_wait;
 #define MAX .15
 #define MIN 0
+#define PI 3.14159265
 
 //#define P_TERM 5
 //#define I_TERM 0
@@ -30,7 +32,7 @@
 {
     float P_TERM = 1;
     float I_TERM = 0;
-    float D_TERM = 0;
+    float D_TERM = 2;
 
     btbee.reset();
     robot.sensor_auto_calibrate();
@@ -52,19 +54,29 @@
     int count = 0;
     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> leftval, rightval, lineposval;
+    vector<float> Uvalue, lineposval;
+    vector<float> sweepData;
     FILE *fp = fopen( "/" FSNAME "/data.txt", "w");
+    FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
     
-    //int check = msc.disk_initialize();
+    //Manual implementation of sweep function
+    float t = 0;
+    float A = 0.1;
+    float f = 19.8/20;
+    float newSetPoint;
     
     
-    //ofstream myFile ("/" FSNAME "/data.txt"); 
-    //myFile << hallo << "\n"; 
+    if (sweep == NULL){
+        robot.printf("Nope.");
+        y = 0;
+    }
 
 
     /* for (int i = 0; i <5; ++i)
@@ -74,6 +86,11 @@
     //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();
@@ -106,11 +123,12 @@
 
             else if (lap == 1 || timer.read() > 5) {
                 robot.stop();
-                robot.printf("Size: %d", rightval.size());
+                robot.printf("Size: %d", Uvalue.size());
                 if(fp != NULL){
-                    for (int i = 0; i < rightval.size(); ++i)
-                        fprintf(fp,"%f %f %f\n",leftval[i], rightval[i], lineposval[i]);
+                    for (int i = 0; i < Uvalue.size(); ++i)
+                        fprintf(fp,"%f %f\n",Uvalue[i], lineposval[i]);
                     fclose(fp);
+                    fclose(sweep);
                     robot.cls();
                     robot.locate(0,0);
                     robot.printf("Doner");
@@ -189,7 +207,20 @@
 
 
         current_pos = robot.line_position();
-        proportional = current_pos;
+       /*if (fgets(sweepValue, 100, sweep) != NULL){
+            if (import < sweepData.size())*/
+                f = 19.8/20 * t + .2;
+                newSetPoint = A*sin(2*PI*f*t);
+                proportional =  newSetPoint + current_pos;
+                
+            /*else
+                break;*/
+        /*}
+        else{
+            robot.printf("Try Again");
+            break;
+        }*/
+        
 
         derivative = current_pos - previous_pos;
 
@@ -226,12 +257,12 @@
         //if (myFile.is_open())
         //{
             //btbee.printf("%f %f %f\n", left, right, robot.line_position());
-            leftval.push_back(left);
-            rightval.push_back(right);
+            Uvalue.push_back(power);
             lineposval.push_back(robot.line_position());
         //}
     
-
+        //++import;
+        t = t + .005;
         wait((5-time_wait.read_ms())/1000);
     }