Diff: main.cpp

Revision:

4:a8dce9e269e5

Parent:

3:4ba3d22e50dc

Child:

5:60560dd127a2

--- a/main.cpp	Thu Mar 12 05:58:00 2015 +0000
+++ b/main.cpp	Thu Mar 12 19:25:23 2015 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "stdlib.h"
 
 DigitalOut clk(PTA13);
 DigitalOut si(PTD4);
@@ -15,6 +16,11 @@
 int peak1;
 int peak2;
 
+int peakLoc1;
+int peakLoc2;
+
+int risEdge;
+
 float currSlope;
 
 float straight = 0.00155f;
@@ -23,12 +29,26 @@
 float hardRight = 0.0018f;
 float slightRight = 0.00165f;
 
+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;
     
     while(1) {
             
-        if(integrationCounter % 461== 0){
+        if(integrationCounter % 151== 0){
             si = 1;
             clk = 1;
             //wait(.00001);
@@ -41,51 +61,65 @@
             approxPos = 0;
             peak1 = 0;
             peak2 = 0;
+            risEdge = 0;
+            peakLoc1 = 0;
+            peakLoc2 = 0;
                 
         }
         else if (integrationCounter > 129){
-            for (int c = 0; c < 128; c++) {
-                if (ADCdata[c+1] < ADCdata[c] && !peak1){
+            minVal = 0;
+            for (int c = 0; c < 126; c++) {
+                if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && !peak1 && !risEdge && !peak2){
                     peak1 = 1;
+                    peakLoc1 = c;
                     minVal = ADCdata[c];
-                } else if (ADCdata[c] < minVal && !peak2){
+                } else if (ADCdata[c] < minVal && peak1 && !peak2){
                     minVal = ADCdata[c];
-                } else if (ADCdata[c+1] < ADCdata[c] && !peak2){
+                } 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;
                 }
             }
-            for (int c = 0; c < 128; c++) {
-                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.05f){
+            
+            for (int c = peakLoc1; c < peakLoc2; c++) {
+                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.01f && ADCdata[c] > 0.1f){
                     slopeAccum += c;
                     slopeCount++;
                 }
             }
+            
             approxPos = (float)slopeAccum/(float)slopeCount;
             
-            if(approxPos > 0 && approxPos <= 64){
-                servo.pulsewidth(hardRight);
-            } else if (approxPos > 64 && approxPos <= 128) {
-                servo.pulsewidth(slightRight);
+            if (peak1 && peak2){
+                if(approxPos > 0 && approxPos <= 32){
+                    if (currDirection != slightLeft){
+                        currDirection = slightLeft;
+                        servo.pulsewidth(slightLeft);
+                    }
+                } else if (approxPos > 32 && approxPos <= 60){
+                    if (currDirection != straight){
+                        currDirection = straight;
+                        servo.pulsewidth(straight);
+                    }
+                } else if (approxPos > 70 && approxPos <= 128){
+                    if (currDirection != slightRight){
+                        currDirection = slightRight;
+                        servo.pulsewidth(slightRight);
+                    }
+            } else if (approxPos < 32){
+                if (currDirection != hardLeft){
+                        currDirection = hardLeft;
+                        servo.pulsewidth(hardLeft);
+                }
             }
-            
-            /*
-            if(approxPos > 0 && approxPos <= 16){
-                servo.pulsewidth(hardRight);
-            } else if (approxPos > 16 && approxPos <= 32) {
-                servo.pulsewidth(slightRight);
-            } else if (approxPos > 32 && approxPos <= 96) {
-                servo.pulsewidth(straight);
-            } else if (approxPos > 96 && approxPos <= 112) {
-                servo.pulsewidth(slightLeft);
-            } else if (approxPos > 112 && approxPos <= 128) {
-                servo.pulsewidth(hardLeft);
-            }
-            */
-            integrationCounter = 460;
+        }
+            integrationCounter = 150;
         }
         else{
             clk = 1;
-            wait(.0000001);
+            wait_us(50);
             ADCdata[integrationCounter - 1] = camData;
             clk = 0;
         }