Revision 7:f7f4308c7ac0, committed 2015-03-20

Comitter:

mawk2311

Date:

Fri Mar 20 05:33:43 2015 +0000

Parent:

6:53d521496469

Commit message:

Have a decent, shitty, line tracking thing going.

Changed in this revision

--- a/main.cpp	Fri Mar 20 04:39:52 2015 +0000
+++ b/main.cpp	Fri Mar 20 05:33:43 2015 +0000
@@ -31,6 +31,7 @@
     while(1) {
             
         if(integrationCounter % 151== 0){
+            //__disable_irq
             si = 1;
             clk = 1;
             //wait(.00001);
@@ -41,49 +42,19 @@
             slopeAccum = 0;
             slopeCount = 0;
             approxPos = 0;
-            /*peak1 = 0;
-            peak2 = 0;
-            risEdge = 0;
-            peakLoc1 = 0;
-            peakLoc2 = 0;
-            peakVal1 = 0;
-            peakVal2 = 0;
-            */
                 
         }
         else if (integrationCounter > 129){
-            minVal = ADCdata[14];
-            for (int c = 15; c < 110; c++) {
-               /* if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && !peak1 && !risEdge && !peak2){
-                    peak1 = 1;
-                    peakLoc1 = c;
-                    peakVal1 = ADCdata[c];
-                    minVal = ADCdata[c];
-                } else if (ADCdata[c] < minVal && peak1 && !peak2){
-                    minVal = ADCdata[c];
-                } else if (ADCdata[c+1] > ADCdata[c] && ADCdata[c+2] > ADCdata[c+1] && peak1 && !peak2){
-                    risEdge = 1;
-                } else if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && peak1 && risEdge && !peak2){
-                    peak2 = 1;
-                    peakLoc2 = c;
-                    peakVal2 = ADCdata[c];
-                }
-            }
-            
-            for (int c = peakLoc1; c < peakLoc2; c++) {
-                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.01f && ADCdata[c] > 0.1f){
-                    slopeAccum += c;
-                    slopeCount++;
-                }
-            }*/
-             if (ADCdata[c] < minVal){
+            minVal = ADCdata[15];
+            for (int c = 15; c < 118; c++) {
+                if (ADCdata[c] < minVal){
                     minVal = ADCdata[c];
                     minLoc = c;
                 }
             }
             
-            for (int c = 15; c < 110; c++) {
-                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.01f && ADCdata[c] > 0.1f){
+            for (int c = 15; c < 118; c++) {
+                if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.04f && ADCdata[c] > 0.1f){
                     slopeAccum += c;
                     slopeCount++;
                 }
@@ -91,18 +62,22 @@
             
             approxPos = (float)slopeAccum/(float)slopeCount;
             
-            if(approxPos > 0 && approxPos <= 40){
+            if(approxPos > 0 && approxPos <= 20){
                     servo.pulsewidth(hardLeft);
-            } else if (approxPos > 40 && approxPos <= 60){
+            } else if (approxPos > 20 && approxPos <= 45){
+                    servo.pulsewidth(slightLeft);
+            } else if (approxPos > 45 && approxPos <= 90){
                 servo.pulsewidth(straight);
-            } else if (approxPos > 70 && approxPos <= 128){
+            } else if (approxPos > 90 && approxPos <= 105){
+                servo.pulsewidth(slightRight);
+            } else if (approxPos > 105 && approxPos <= 128){
                     servo.pulsewidth(hardRight);
             }
             integrationCounter = 150;
         }
         else{
             clk = 1;
-            wait_us(60);
+            wait_us(70);
             ADCdata[integrationCounter - 1] = camData;
             clk = 0;
         }