Revision 14:5b3cc7741bdf, committed 2015-06-01

Comitter:

charwhit

Date:

Mon Jun 01 09:33:43 2015 +0000

Parent:

13:9bb4ac4968d2

Child:

15:3e3f1b42cc23

Commit message:

Ready for closed loop testing

Changed in this revision

--- 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);
     }