Revision 16:3e3f1b42cc23, committed 2015-06-01

Comitter:

charwhit

Date:

Mon Jun 01 11:25:46 2015 +0000

Parent:

15:5b3cc7741bdf

Commit message:

Closed Loop experiment code

Changed in this revision

diff -r 5b3cc7741bdf -r 3e3f1b42cc23 main.cpp
--- a/main.cpp	Mon Jun 01 09:33:43 2015 +0000
+++ b/main.cpp	Mon Jun 01 11:25:46 2015 +0000
@@ -18,7 +18,7 @@
 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
 Timer timer;
 Timer time_wait;
-#define MAX .35
+#define MAX .95
 #define MIN 0
 #define PI 3.14159265
 
@@ -30,9 +30,9 @@
 
 int main()
 {
-    float P_TERM = 1;
-    float I_TERM = 0;
-    float D_TERM = 0;
+    float P_TERM = 2.5;
+    float I_TERM = .5;
+    float D_TERM = 25;
 
     btbee.reset();
     robot.sensor_auto_calibrate();
@@ -64,16 +64,16 @@
     vector<float> Uvalue, lineposval, setPointVals;
     vector<float> sweepData;
     FILE *fp = fopen( "/" FSNAME "/data.txt", "w");
-    FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
+    //FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
     
     //Manual implementation of sweep function
    
     
     
-    if (sweep == NULL){
+    /*if (sweep == NULL){
         robot.printf("Nope.");
         y = 0;
-    }
+    }*/
 
 
     /* for (int i = 0; i <5; ++i)
@@ -119,13 +119,14 @@
             }
 
             else if (lap == 1) {
+                robot.printf("Success.");
                 robot.stop();
                 robot.printf("Size: %d", Uvalue.size());
                 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);
+                    //fclose(sweep);
                     robot.cls();
                     robot.locate(0,0);
                     robot.printf("Doner");
@@ -157,6 +158,7 @@
                 count = 0;
                 timer.stop();
                 timer.reset();
+                y = 0;
                 continue;
                 
             } else {
@@ -204,15 +206,16 @@
 
 
         current_pos = robot.line_position();
-       if (fgets(sweepValue, 100, sweep) != NULL){
+       //if (fgets(sweepValue, 100, sweep) != NULL){
     
-                proportional =  atof(sweepValue) + current_pos;
+               // proportional =  atof(sweepValue) + current_pos;
+               proportional =  current_pos;
                 
-        }
+       /* }
         else{
             fclose(sweep);
             FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
-        }
+        }*/
         
 
         derivative = current_pos - previous_pos;
@@ -252,7 +255,7 @@
             //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));
+            setPointVals.push_back(0);
         //}