Revision 17:5eb563904361, committed 2015-06-10

Comitter:

bbabbs

Date:

Wed Jun 10 13:20:43 2015 +0000

Parent:

16:caa77287cc25

Commit message:

one lap kind of works

Changed in this revision

--- a/main.cpp	Wed Jun 10 12:01:21 2015 +0000
+++ b/main.cpp	Wed Jun 10 13:20:43 2015 +0000
@@ -32,8 +32,12 @@
 {
     float P_TERM = 3;
     float I_TERM = 0;
-    float D_TERM = 1;
-
+    float D_TERM = 27;
+    int last_lap = 0;
+    int position = 0;
+    float llpp = 0; //last lap previous point
+    float llnp = 0; //last lap next point
+    float llcp = 0; //last lap current point
     btbee.reset();
     robot.sensor_auto_calibrate();
     wait(2.0);
@@ -60,7 +64,7 @@
 
     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> Uvalue, lineposval, setPointVals, Currentvec;
     vector<float> sweepData;
     FILE *fp = fopen( "/" FSNAME "/exper1.txt", "w");
     //FILE *sweep = fopen("/" FSNAME "/sweep15.txt","r");
@@ -78,7 +82,7 @@
 
 
     //wait(8);
-    btbee.printf("Battery: %f\n", robot.battery());
+   // btbee.printf("Battery: %f\n", robot.battery());
 
 
     //btbee.printf("C: %d", sweepData.size());
@@ -133,11 +137,12 @@
                 lap_time = timer.read();
                 total_time += lap_time;
                 average_time = total_time/lap;
+                robot.cls();
                 robot.printf("%f",average_time);
-                if (btbee.writeable()) {
-                    btbee.printf("Lap %d time: %f\n", lap, lap_time);
-                    btbee.printf("Avg Lap time: %f\n", average_time);
-                }
+     //           if (btbee.writeable()) {
+       //             btbee.printf("Lap %d time: %f\n", lap, lap_time);
+         //           btbee.printf("Avg Lap time: %f\n", average_time);
+           //     }
 
                 /*while (count < 3) {
                     btbee.read_line(arr_read, 30, &chars_read);
@@ -156,20 +161,24 @@
                 timer.stop();
                 timer.reset();
                 continue;*/
-                break;
+           //     break;
+                
+
 
             } else {
                 /*while( x[0]> 300 && x[4] > 300) {
                     robot.calibrated_sensor(x);
                 }*/
                 lap_time = timer.read();
-                if (btbee.writeable()) {
-                    btbee.printf("Lap %d time: %f\n", lap, lap_time);
-                }
+             //   if (btbee.writeable()) {
+               //     btbee.printf("Lap %d time: %f\n", lap, lap_time);
+               // }
                 total_time += lap_time;
                 average_time = total_time/lap;
                 lap = lap +1;
                 timer.reset();
+                robot.cls();
+                robot.print("%f", lap_time);
             }
             passed = true;
         } else if (x[0] > 300 && x[2]>300 && x[4]>300)
@@ -237,9 +246,31 @@
 
         //remember the last position.
         previous_pos = current_pos;
+        if(last_lap != lap){
+            position = 0;
+            llpp = llcp = llnp = 0;
+        }
+        last_lap = lap;
 
         // compute the power
-        power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+        if(lap == 1){
+            power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+            Uvalue.push_back(power);
+            Currentvec.push_back(current_pos);
+        }
+        else if(lap == 0){
+            power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
+        }
+        else{
+            llpp = llcp;
+            llcp = llnp;
+            llnp = Currentvec[position + 1];
+            power = Uvalue[position] + (P_TERM)*llcp + (D_TERM*(llpp + llnp) / .01);
+            Uvalue[position] = power;
+            Currentvec[position] = llcp;
+            position++;
+        }
+        
         /*if (fgets(sweepValue, 100, sweep) != NULL){
             power =  atof(sweepValue) + power;
         }
@@ -278,9 +309,10 @@
 
 
 
-        Uvalue.push_back(power);
-        lineposval.push_back(robot.line_position());
-        setPointVals.push_back(0);
+       // Uvalue.push_back(power);
+       // Currentvec.push_back(current_pos);
+//        lineposval.push_back(robot.line_position());
+//        setPointVals.push_back(0);
         //setPointVals.push_back(0.5);