Diff: main.cpp

Revision:

3:4ba3d22e50dc

Parent:

2:f3eafd4d3705

Child:

4:a8dce9e269e5

--- a/main.cpp	Wed Mar 11 23:46:55 2015 +0000
+++ b/main.cpp	Thu Mar 12 05:58:00 2015 +0000
@@ -3,28 +3,90 @@
 DigitalOut clk(PTA13);
 DigitalOut si(PTD4);
 AnalogIn camData(PTC2);
-Timer t1;
+PwmOut servo(PTA5);
+Serial pc(USBTX, USBRX); // tx, rx
+
+float ADCdata [128];
+float slopeAccum;
+float slopeCount;
+float approxPos;
+float minVal;
+
+int peak1;
+int peak2;
+
+float currSlope;
+
+float straight = 0.00155f;
+float hardLeft = 0.0013f;
+float slightLeft = 0.00145f;
+float hardRight = 0.0018f;
+float slightRight = 0.00165f;
+
 int main() {
-    //clk.period_us(2);
-    //clk.pulsewidth_us(1);
-    t1.start();
-    //int integrationTime = 100 ;//SET THIS!!
     int integrationCounter = 0;
+    
     while(1) {
-        //int time = t1.read_us();
-        //(t1.read_us()>integrationTime){
-        if(integrationCounter % 120== 0){
+            
+        if(integrationCounter % 461== 0){
             si = 1;
             clk = 1;
             //wait(.00001);
             si = 0;
             clk = 0;
-            //integrationTime = time;
-            t1.reset();
-            //integrationCounter = 0;
+            integrationCounter = 0;
+            
+            slopeAccum = 0;
+            slopeCount = 0;
+            approxPos = 0;
+            peak1 = 0;
+            peak2 = 0;
+                
+        }
+        else if (integrationCounter > 129){
+            for (int c = 0; c < 128; c++) {
+                if (ADCdata[c+1] < ADCdata[c] && !peak1){
+                    peak1 = 1;
+                    minVal = ADCdata[c];
+                } else if (ADCdata[c] < minVal && !peak2){
+                    minVal = ADCdata[c];
+                } else if (ADCdata[c+1] < ADCdata[c] && !peak2){
+                    peak2 = 1;
+                }
+            }
+            for (int c = 0; c < 128; c++) {
+                if (ADCdata[c] > minVal && ADCdata[c] - minVal < 0.05f){
+                    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(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;
         }
         else{
             clk = 1;
+            wait(.0000001);
+            ADCdata[integrationCounter - 1] = camData;
             clk = 0;
         }