Revision 22:1288bd38d682, committed 2015-06-18

Comitter:

charwhit

Date:

Thu Jun 18 12:12:13 2015 +0000

Parent:

21:df402d79b2ad

Commit message:

Commented final code

Changed in this revision

diff -r df402d79b2ad -r 1288bd38d682 main.cpp
--- a/main.cpp	Fri Jun 12 09:52:22 2015 +0000
+++ b/main.cpp	Thu Jun 18 12:12:13 2015 +0000
@@ -12,7 +12,6 @@
 
 
 MSCFileSystem msc(FSNAME); // Mount flash drive under the name "msc"
-//Serial pc(USBTX,USBRX);
 
 m3pi robot;
 btbee btbee;
@@ -20,7 +19,7 @@
 DigitalIn m3pi_pb(p21);
 Timer timer;
 Timer time_wait;
-#define MAX 1
+#define MAX .96
 #define MIN 0
 #define PI 3.14159265
 
@@ -29,6 +28,7 @@
 //#define D_TERM 20
 
 
+//Struct that represents test points in Nedler-Mead. Of the form (P,D) i.e (P=5,D=7)
 struct testPoint {
     testPoint():Pvalue(0), Dvalue(0), time(1000) {}
     testPoint(float a, float b, float c): Pvalue(a), Dvalue(b), time(c) {}
@@ -66,6 +66,7 @@
 };
 
 
+//Comparator for testPoint struct sorting
 bool myComp (testPoint i,testPoint j)
 {
     return (i.time<j.time);
@@ -79,21 +80,20 @@
     float P_TERM = 3;
     float I_TERM = 0;
     float D_TERM = 27;
-    //int last_lap = 0;
-    //int position = 0;
+    
 
     m3pi_pb.mode(PullUp);
     btbee.reset();
 
 
     robot.sensor_auto_calibrate();
+    //Waiting for button to be pressed before starting for bluetooth comms
     while(m3pi_pb) {
 
     }
     wait(2.0);
     float right;
     float left;
-    //float current_pos[5];
     float current_pos = 0.0;
     float previous_pos =0.0;
     float derivative, proportional, integral = 0;
@@ -103,12 +103,10 @@
     int lap = 0;
     float lap_time = 0.0;
     int y =1;
-    int count = 0;
 
     bool passed = false;
 
     Timer crossProtect;
-    bool poop = false; //Random variable so that it doesn't print 50 gagillion times
 
 
     //Simplex algorithm stuff
@@ -130,41 +128,24 @@
     float expanFactor = 2;
     float contrFactor = -1/2;
     float reducFactor = 1/2;
-    Timer backup;
+    Timer backup; //Timer so robot doesnt count a lap when it swerves wierdly
+    
+    //Max and min values for P and D parameters
     int MINPVAL = 2;
     int MAXPVAL = 30;
-    int MAXDVAL= 50;
-
-
-
-    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, Currentvec;
-    vector<float> sweepData;
-    FILE *fp = fopen( "/" FSNAME "/exper1.txt", "w");
-
-
-
-    //wait(8);
-    // btbee.printf("Battery: %f\n", robot.battery());
-
-
+    int MAXDVAL= 40;
 
 
 
     time_wait.start();
     backup.start();
     int x [5];
-    //timer.start();
     while(y) {
         time_wait.reset();
-        //timer.reset();
+        
         //Get raw sensor values
-
         robot.calibrated_sensor(x);
 
-
-
         //Check to make sure battery isn't low
         if (robot.battery() < 2.4) {
             timer.stop();
@@ -173,22 +154,20 @@
             break;
         }
 
-        //else if (m3pi_IN [0] == 0)
-        //{break;}
+        
 
         //running the four corners
         if(x[0] > 300 && x[2]>300 && x[4]>300 && !passed && (backup.read() > 2 || lap == 0)) {
             btbee.printf("Crossing\n");
-            if (lap == 0) {
+            if (lap == 0) { //Allows for 10cm head start
                 timer.start();
-                //crossProtect.start();
                 lap= lap +1;
-            } else if(corners < 2) {
+            } else if(corners < 2) { // This condition get lap times for the intial group of 3 points
                 lap_time = timer.read();
                 triangle[corners].time = lap_time;
-                //if (btbee.writeable()) {
+                
                 btbee.printf("Lap %d time: %f\n", lap, lap_time);
-                //}
+                
 
                 ++corners;
                 robot.cls();
@@ -203,9 +182,9 @@
                     triangle[corners].time = timer.read();
                     ++corners;
                 }
-                switch(step) {
+                switch(step) { //Each step corresponds to a step in Nedler-Mead
                     case 1:
-                        if (triangle[1].time == 1000 || triangle[2].time == 1000){
+                        if (triangle[1].time == 1000 || triangle[2].time == 1000){ //Getting new data if reduced
                             if (triangle[1].time == 1000){
                                 triangle[1].time = timer.read();
                                 P_TERM = triangle[2].Pvalue;
@@ -347,10 +326,7 @@
 
         // compute the power
         power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
-        /*if (poop){
-            btbee.printf("Check: P: %f D:%f Power: %f\n", P_TERM, D_TERM, power);
-            poop = false;
-        }*/
+
 
         //computer new speeds
         right = speed+power;
@@ -368,12 +344,9 @@
             left = MAX;
 
         //set speed
-
         robot.left_motor(left);
         robot.right_motor(right);
 
-        //traveled += ((right + left) / 2) * .005;
-
 
         wait((5-time_wait.read_ms())/1000);
     }
@@ -382,16 +355,5 @@
 
     robot.stop();
 
-    /*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);*/
-
-
-
 
 }
\ No newline at end of file