Revision 16:caa77287cc25, committed 2015-06-10

Comitter:

charwhit

Date:

Wed Jun 10 12:01:21 2015 +0000

Parent:

15:3e3f1b42cc23

Commit message:

Course code without bluetooth

Changed in this revision

diff -r 3e3f1b42cc23 -r caa77287cc25 main.cpp
--- a/main.cpp	Mon Jun 01 11:25:46 2015 +0000
+++ b/main.cpp	Wed Jun 10 12:01:21 2015 +0000
@@ -18,7 +18,7 @@
 DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
 Timer timer;
 Timer time_wait;
-#define MAX .95
+#define MAX .9
 #define MIN 0
 #define PI 3.14159265
 
@@ -30,9 +30,9 @@
 
 int main()
 {
-    float P_TERM = 2.5;
-    float I_TERM = .5;
-    float D_TERM = 25;
+    float P_TERM = 3;
+    float I_TERM = 0;
+    float D_TERM = 1;
 
     btbee.reset();
     robot.sensor_auto_calibrate();
@@ -55,22 +55,19 @@
     float paramChange[3];
     bool passed = false;
     char sweepValue[100];
-  
-    //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, setPointVals;
     vector<float> sweepData;
-    FILE *fp = fopen( "/" FSNAME "/data.txt", "w");
-    //FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
-    
-    //Manual implementation of sweep function
-   
-    
-    
-    /*if (sweep == NULL){
+    FILE *fp = fopen( "/" FSNAME "/exper1.txt", "w");
+    //FILE *sweep = fopen("/" FSNAME "/sweep15.txt","r");
+
+
+
+    /*if (sweep == NULL) {
         robot.printf("Nope.");
         y = 0;
     }*/
@@ -78,23 +75,23 @@
 
     /* for (int i = 0; i <5; ++i)
          current_pos[i] = 0.0; */
-    
-    
+
+
     //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());
+
+
+    //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
-        int x [5];
+
         robot.calibrated_sensor(x);
 
 
@@ -102,13 +99,14 @@
         //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) {
                 /*while( x[0]> 300 && x[4] > 300) {
@@ -119,18 +117,19 @@
             }
 
             else if (lap == 1) {
-                robot.printf("Success.");
+
                 robot.stop();
-                robot.printf("Size: %d", Uvalue.size());
-                if(fp != NULL){
+                
+                //Print to file stuff
+                /*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.locate(0,0);
                     robot.printf("Doner");
                 }
+                break;*/
                 lap_time = timer.read();
                 total_time += lap_time;
                 average_time = total_time/lap;
@@ -139,12 +138,10 @@
                     btbee.printf("Lap %d time: %f\n", lap, lap_time);
                     btbee.printf("Avg Lap time: %f\n", average_time);
                 }
-                
-                while (count < 3){
-                    //btbee.printf("Input parameter\n");
+
+                /*while (count < 3) {
                     btbee.read_line(arr_read, 30, &chars_read);
                     paramChange[count] = atof(arr_read);
-                    //btbee.printf("%d", arr_read);
                     count++;
                 }
                 P_TERM = paramChange[0];
@@ -158,9 +155,9 @@
                 count = 0;
                 timer.stop();
                 timer.reset();
-                y = 0;
-                continue;
-                
+                continue;*/
+                break;
+
             } else {
                 /*while( x[0]> 300 && x[4] > 300) {
                     robot.calibrated_sensor(x);
@@ -172,15 +169,14 @@
                 total_time += lap_time;
                 average_time = total_time/lap;
                 lap = lap +1;
-                //timer.reset();
+                timer.reset();
             }
             passed = true;
-        }
-        else if (x[0] > 300 && x[2]>300 && x[4]>300)
+        } else if (x[0] > 300 && x[2]>300 && x[4]>300)
             passed = true;
         else
             passed = false;
-        
+
 
 
         // Get the position of the line.
@@ -206,17 +202,32 @@
 
 
         current_pos = robot.line_position();
-       //if (fgets(sweepValue, 100, sweep) != NULL){
-    
-               // proportional =  atof(sweepValue) + current_pos;
-               proportional =  current_pos;
-                
-       /* }
-        else{
-            fclose(sweep);
-            FILE *sweep = fopen("/" FSNAME "/sweep1.txt","r");
+
+        //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;
 
@@ -229,6 +240,22 @@
 
         // compute the power
         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;
@@ -248,18 +275,16 @@
 
         robot.left_motor(left);
         robot.right_motor(right);
-        
-        
-        //if (myFile.is_open())
-        //{
-            //btbee.printf("%f %f %f\n", left, right, robot.line_position());
-            Uvalue.push_back(power);
-            lineposval.push_back(robot.line_position());
-            setPointVals.push_back(0);
-        //}
-    
-        
-       
+
+
+
+        Uvalue.push_back(power);
+        lineposval.push_back(robot.line_position());
+        setPointVals.push_back(0);
+        //setPointVals.push_back(0.5);
+
+
+
         wait((5-time_wait.read_ms())/1000);
     }
 
@@ -267,14 +292,14 @@
 
     robot.stop();
 
-    char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
+    /*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);
+    robot.playtune(hail,numb);*/