E=MC
/
e=mc
.
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"); } }