Diff: main.cpp

Revision:

12:48b76450c4b4

Parent:

11:b59ec039a712

Child:

13:5be515371946

diff -r b59ec039a712 -r 48b76450c4b4 main.cpp
--- a/main.cpp	Thu Apr 09 01:13:49 2015 +0000
+++ b/main.cpp	Thu Apr 09 05:07:19 2015 +0000
@@ -24,10 +24,12 @@
 float maxAccum;
 float maxCount;
 float approxPos;
+float prevApproxPos;
+int trackWindow = 20;
 float maxVal;
 int maxLoc;
-int maxSlopeLoc [5];
-int minSlopeLoc [5];
+
+bool firstTime;
 
 //Brightness accommodater
 int intTimMod = 0;
@@ -43,8 +45,10 @@
 
 //Servo turning parameters
 float straight = 0.00155f;
-float hardLeft = 0.0010f;
-float hardRight = 0.00195f;
+float hardLeft = 0.0011f;
+float hardRight = 0.00185f;
+//float hardLeft = 0.0010f;
+//float hardRight = 0.00195f;
 
 //Servo Directions
 float currDir;
@@ -80,7 +84,7 @@
 
 int numInterrupts = 0;
 
-float pulsewidth = 0.25f;
+float pulsewidth = 0.2f;
 
 // Hardware periods
 float motorPeriod = .0025;
@@ -310,11 +314,13 @@
     
     wait(3);
 
-    //motor1.pulsewidth(motorPeriod*pulsewidth);
-    //motor2.pulsewidth(motorPeriod*pulsewidth);
+    motor1.pulsewidth(motorPeriod*pulsewidth);
+    motor2.pulsewidth(motorPeriod*pulsewidth);
     break1 = 0;
     break2 = 0;
     
+    firstTime = true;
+    
     //t.start();
     
     if(dataCol){
@@ -351,8 +357,6 @@
             maxCount = 0;
             approxPos = 0;
             
-            maxSlopeLoc[0] = 0;
-            maxSlopeLoc[0] = 0;
             space = false;
 
         }
@@ -364,16 +368,40 @@
             //interrupt.fall(&fallInterrupt);
             //interrupt.rise(&riseInterrupt);
             
-            maxVal = ADCdata[10];
-            for (int c = 11; c < 118; c++) {
-                if (ADCdata[c] > maxVal){
-                    maxVal = ADCdata[c];
-                    maxLoc = c;
+            if (firstTime){
+            
+                maxVal = ADCdata[10];
+                for (int c = 11; c < 118; c++) {
+                    if (ADCdata[c] > maxVal){
+                        maxVal = ADCdata[c];
+                        maxLoc = c;
+                    }
                 }
-            }
-            
-            for (int c = 10; c < 118; c++) {
-                    if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.005f /*&& ADCdata[c] > 0.1f*/){
+                
+                for (int c = 10; c < 118; c++) {
+                        if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.025f){
+                            maxAccum += c;
+                            maxCount++;
+                            if (c > prevTrackLoc + spaceThresh){
+                                space = true;
+                            }
+                            prevTrackLoc = c;
+                        }
+                }
+                
+                firstTime = false;
+            } else {
+                
+                maxVal = ADCdata[10];
+                for (int c = prevApproxPos - trackWindow; c < prevApproxPos + trackWindow; c++) {
+                    if (ADCdata[c] > maxVal){
+                        maxVal = ADCdata[c];
+                        maxLoc = c;
+                    }
+                }
+                
+                for (int c = prevApproxPos - trackWindow; c < prevApproxPos + trackWindow; c++) {
+                    if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.025f){
                         maxAccum += c;
                         maxCount++;
                         if (c > prevTrackLoc + spaceThresh){
@@ -381,8 +409,9 @@
                         }
                         prevTrackLoc = c;
                     }
+                }
             }
-            
+            /*
             //Check if we need to alter integration time due to brightness
             if (maxVal < 0.15f){
                 intTimMod += 10;
@@ -391,11 +420,12 @@
                     intTimMod -= 10;
                 }
             }
-            
+            */
             
             //Line Crossing Checks
             if (space) {
                 currDir = prevDir;
+                firstTime = true;
             } else {
                 approxPos = (float)maxAccum/(float)maxCount;
 
@@ -408,7 +438,7 @@
                 }
                 
                 currDir = hardLeft + (approxPos - 10)/((float) 108)*(hardRight-hardLeft);
-                
+                prevApproxPos = approxPos;
 
             }