Diff: main.cpp

Revision:

6:53d521496469

Parent:

5:60560dd127a2

Child:

7:f7f4308c7ac0

--- a/main.cpp	Thu Mar 12 19:47:11 2015 +0000
+++ b/main.cpp	Fri Mar 20 04:39:52 2015 +0000
@@ -1,7 +1,8 @@
 #include "mbed.h"
 #include "stdlib.h"
 
-DigitalOut clk(PTA13);
+DigitalOut led1(LED1);
+DigitalOut clk(PTD5);
 DigitalOut si(PTD4);
 AnalogIn camData(PTC2);
 PwmOut servo(PTA5);
@@ -13,18 +14,7 @@
 float approxPos;
 float minVal;
 
-float peakVal1;
-float peakVal2;
-
-int peak1;
-int peak2;
-
-int peakLoc1;
-int peakLoc2;
-
-int risEdge;
-
-float currSlope;
+int minLoc;
 
 float straight = 0.00155f;
 float hardLeft = 0.0013f;
@@ -51,19 +41,20 @@
             slopeAccum = 0;
             slopeCount = 0;
             approxPos = 0;
-            peak1 = 0;
+            /*peak1 = 0;
             peak2 = 0;
             risEdge = 0;
             peakLoc1 = 0;
             peakLoc2 = 0;
             peakVal1 = 0;
             peakVal2 = 0;
+            */
                 
         }
         else if (integrationCounter > 129){
-            minVal = 0;
-            for (int c = 0; c < 126; c++) {
-                if (ADCdata[c+1] < ADCdata[c] && ADCdata[c+2] < ADCdata[c+1] && !peak1 && !risEdge && !peak2){
+            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];
@@ -84,32 +75,34 @@
                     slopeAccum += c;
                     slopeCount++;
                 }
+            }*/
+             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){
+                    slopeAccum += c;
+                    slopeCount++;
+                }
             }
             
             approxPos = (float)slopeAccum/(float)slopeCount;
             
-            if (peak1 && peak2){
-                if(approxPos > 0 && approxPos <= 40){
-                    if (peakVal2 - peakVal1 > 0.65f){
-                        servo.pulsewidth(hardLeft);
-                    } else {
-                        servo.pulsewidth(slightLeft);
-                    }
-                } else if (approxPos > 40 && approxPos <= 60){
-                    servo.pulsewidth(straight);
-                } else if (approxPos > 70 && approxPos <= 128){
-                    if (peakVal1 - peakVal2 > 0.8f){
-                        servo.pulsewidth(hardRight);
-                    } else {
-                        servo.pulsewidth(slightRight);
-                    }
-                }
+            if(approxPos > 0 && approxPos <= 40){
+                    servo.pulsewidth(hardLeft);
+            } else if (approxPos > 40 && approxPos <= 60){
+                servo.pulsewidth(straight);
+            } else if (approxPos > 70 && approxPos <= 128){
+                    servo.pulsewidth(hardRight);
             }
             integrationCounter = 150;
         }
         else{
             clk = 1;
-            wait_us(50);
+            wait_us(60);
             ADCdata[integrationCounter - 1] = camData;
             clk = 0;
         }