Diff: main.cpp

Revision:

15:d746c53bf49b

Parent:

14:bda4a189cbe8

Child:

16:f7ab57048e1e

--- a/main.cpp	Thu Apr 23 23:29:16 2015 +0000
+++ b/main.cpp	Fri Apr 24 00:24:36 2015 +0000
@@ -126,7 +126,7 @@
 
 //PIDcontrol -----------------------------------------------------
 int centerSum = 0;
-int prevLineCenter = 0;
+float prevLineCenter = 0.0f;
 int prevTime = 0;
 int p,i,d = 1;
 float timeDiff = 0.0f;
@@ -154,11 +154,15 @@
 
 float PIDcontrol(int lineCenter){
     //centerSum += lineCenter;
+    float newLineCenter = (float)lineCenter/127.0f;
     float newTime = t.read();
     timeDiff = newTime - lastTime;
     lastTime = newTime; 
-    float lineDif = ((float)prevLineCenter - (float)lineCenter)/(float) timeDiff;
-    return p * lineCenter + d * lineDif;
+    float lineDif = ((float)newLineCenter - (float)prevLineCenter)/(float) timeDiff;
+    if(prevLineCenter != newLineCenter){
+        prevLineCenter = newLineCenter;
+    }
+    return p * newLineCenter + d * lineDif;
     
 }
 
@@ -308,11 +312,11 @@
                 approxPos = (float)maxAccum/(float)maxCount;
 
                 if(dataCol){
-                    if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%6==0){
+                    if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%8==0){
                         lineCenters[loopCtr] = approxPos;
                         times[loopCtr] = printTimer.read_ms();
                         pSteering[loopCtr] = currDir;
-                        pSteering[loopCtr] = PIDcontrol(approxPos);
+                        pdSteering[loopCtr] = PIDcontrol(approxPos);
                         loopCtr++;
                     }
                 }