Diff: main.cpp

Revision:

16:f7ab57048e1e

Parent:

15:d746c53bf49b

Child:

17:054a54d73e04

--- a/main.cpp	Fri Apr 24 00:24:36 2015 +0000
+++ b/main.cpp	Fri Apr 24 01:10:31 2015 +0000
@@ -64,6 +64,8 @@
 int times [numData] = {0};
 float pSteering [numData] = {0.0f};
 float pdSteering [numData] = {0.0f};
+float lineDifData [numData] = {0.0f};
+float pdLineCenter [numData] = {0.0f};
 int loopCtr = 0;
 
 //End of Line Tracking Variables -------------------------
@@ -128,7 +130,7 @@
 int centerSum = 0;
 float prevLineCenter = 0.0f;
 int prevTime = 0;
-int p,i,d = 1;
+float p,i,d = 1.0f;
 float timeDiff = 0.0f;
 float lastTime = 0.0f;
 
@@ -152,13 +154,14 @@
 
 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-float PIDcontrol(int lineCenter){
+float PIDcontrol(int lineCenter, int loopCtr){
     //centerSum += lineCenter;
     float newLineCenter = (float)lineCenter/127.0f;
     float newTime = t.read();
     timeDiff = newTime - lastTime;
     lastTime = newTime; 
     float lineDif = ((float)newLineCenter - (float)prevLineCenter)/(float) timeDiff;
+    lineDifData[loopCtr] = lineDif;
     if(prevLineCenter != newLineCenter){
         prevLineCenter = newLineCenter;
     }
@@ -305,25 +308,32 @@
             */
             
             //Line Crossing Checks
-            if (space) {
-                currDir = prevDir;
-                firstTime = true;
-            } else {
-                approxPos = (float)maxAccum/(float)maxCount;
-
-                if(dataCol){
+            if(dataCol){
+                if (space) {
+                    currDir = prevDir;
                     if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){
                         lineCenters[loopCtr] = approxPos;
                         times[loopCtr] = printTimer.read_ms();
                         pSteering[loopCtr] = currDir;
-                        pdSteering[loopCtr] = PIDcontrol(approxPos);
+                        pdSteering[loopCtr] = PIDcontrol(approxPos, loopCtr);
                         loopCtr++;
                     }
+                    firstTime = true;
+                } else {
+                    approxPos = (float)maxAccum/(float)maxCount;
+                    currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft);
+                    if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){
+                        lineCenters[loopCtr] = approxPos;
+                        times[loopCtr] = printTimer.read_ms();
+                        pSteering[loopCtr] = currDir;
+                        pdSteering[loopCtr] = PIDcontrol(approxPos, loopCtr);
+                        loopCtr++;
+                    }
+                    
+                    
+                    prevApproxPos = approxPos;
+    
                 }
-                
-                currDir = hardLeft + (approxPos)/((float) 127)*(hardRight-hardLeft);
-                prevApproxPos = approxPos;
-
             }
 
             servo.pulsewidth(currDir);
@@ -366,13 +376,18 @@
         pc.printf("printing frame data\n\r");
         //int frameSize = frames.size();
         //pc.printf("%i",frameSize);
-        pc.printf("[");
-        for(int i=0; i<numData; i++){
+        for(int i=0; i<numData; i+=10){
             if(lineCenters > 0){
-                pc.printf("%i %i %f %f,",lineCenters[i], times[i], pSteering[i], pdSteering[i]);
+                pc.printf("LINE #: %i\n\r",i);
+                pc.printf("line center: %i\n\r", lineCenters[i]);
+                pc.printf("time: %i\n\r", times[i]);
+                pc.printf("pSteer: %f\n\r", pSteering[i]);
+                pc.printf("pdSteer: %f\n\r", pdSteering[i]);
+                pc.printf("lineDif: %f\n\r", lineDifData[i]);
+                pc.printf("pdLineCenter: %f\n\r", pdLineCenter[i]);
+                //pc.printf("%i %i %f %f,",lineCenters[i], times[i], pSteering[i], pdSteering[i]);
             }
         }
-        pc.printf("]\n\r");
     }
 }