Diff: main.cpp

Revision:

5:60560dd127a2

Parent:

4:a8dce9e269e5

Child:

6:53d521496469

--- a/main.cpp	Thu Mar 12 19:25:23 2015 +0000
+++ b/main.cpp	Thu Mar 12 19:47:11 2015 +0000
@@ -13,6 +13,9 @@
 float approxPos;
 float minVal;
 
+float peakVal1;
+float peakVal2;
+
 int peak1;
 int peak2;
 
@@ -31,17 +34,6 @@
 
 float currDirection = straight;
 
-int comp (const void * elem1, const void * elem2) 
-{
-    int f = *((int*)elem1);
-    int s = *((int*)elem2);
-    if (f > s) return  1;
-    if (f < s) return -1;
-    return 0;
-}
-
-
-
 int main() {
     //servo.period(SERVO_FREQ);
     int integrationCounter = 0;
@@ -64,6 +56,8 @@
             risEdge = 0;
             peakLoc1 = 0;
             peakLoc2 = 0;
+            peakVal1 = 0;
+            peakVal2 = 0;
                 
         }
         else if (integrationCounter > 129){
@@ -72,6 +66,7 @@
                 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];
@@ -80,6 +75,7 @@
                 } else if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && peak1 && risEdge && !peak2){
                     peak2 = 1;
                     peakLoc2 = c;
+                    peakVal2 = ADCdata[c];
                 }
             }
             
@@ -93,28 +89,22 @@
             approxPos = (float)slopeAccum/(float)slopeCount;
             
             if (peak1 && peak2){
-                if(approxPos > 0 && approxPos <= 32){
-                    if (currDirection != slightLeft){
-                        currDirection = slightLeft;
+                if(approxPos > 0 && approxPos <= 40){
+                    if (peakVal2 - peakVal1 > 0.65f){
+                        servo.pulsewidth(hardLeft);
+                    } else {
                         servo.pulsewidth(slightLeft);
                     }
-                } else if (approxPos > 32 && approxPos <= 60){
-                    if (currDirection != straight){
-                        currDirection = straight;
-                        servo.pulsewidth(straight);
-                    }
+                } else if (approxPos > 40 && approxPos <= 60){
+                    servo.pulsewidth(straight);
                 } else if (approxPos > 70 && approxPos <= 128){
-                    if (currDirection != slightRight){
-                        currDirection = slightRight;
+                    if (peakVal1 - peakVal2 > 0.8f){
+                        servo.pulsewidth(hardRight);
+                    } else {
                         servo.pulsewidth(slightRight);
                     }
-            } else if (approxPos < 32){
-                if (currDirection != hardLeft){
-                        currDirection = hardLeft;
-                        servo.pulsewidth(hardLeft);
                 }
             }
-        }
             integrationCounter = 150;
         }
         else{