Diff: main.cpp

Revision:

19:29b3decea5e2

Parent:

18:16f1297a4260

Child:

20:53f5e6d3e47b

--- a/main.cpp	Fri Mar 20 05:53:29 2015 +0000
+++ b/main.cpp	Fri Mar 20 08:54:51 2015 +0000
@@ -15,7 +15,7 @@
 Serial pc(USBTX, USBRX);
 // Interrupt for encoder
 InterruptIn interrupt(PTA13);
-const int maxFrames = 10;
+const int maxFrames = 3;
 float frames[maxFrames][128];
 float ADCdata [128];
 Mutex line_mutex;
@@ -34,11 +34,71 @@
     
 }
 
+int find_track(float line[]){
+    int track_location = -1;
+    float slope_threshold = .05;
+    bool downslope_indices [128] = {false};
+    bool upslope_indices [128] = {false};
+    for(int i=10; i<118; i++){
+        if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
+            downslope_indices[i] = true;
+        }
+        if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
+            upslope_indices[i] = true;
+        }
+    }
+    int numDownslopes = 0;
+    int numUpslopes = 0;
+    for(int i=0; i<128; i++){
+        if(downslope_indices[i] == true){
+            numDownslopes ++;
+        }
+        if(upslope_indices[i] == true){
+            numUpslopes ++;
+        }
+    }
+    int downslope_locs [numDownslopes];
+    int upslope_locs [numUpslopes];
+    int dsctr = 0;
+    int usctr = 0;
+    
+    for (int i=0; i<128; i++){
+        if(downslope_indices[i] == true){
+            downslope_locs[dsctr] = i;
+            dsctr++;
+        }
+        if(upslope_indices[i] == true){
+            upslope_locs[usctr] = i;
+            usctr++;
+        }
+    }
+    
+    for(int i=0; i<numDownslopes; i++){
+        for(int j=0; j<numUpslopes; j++){
+            if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
+                track_location = downslope_locs[i] + 2 ;
+            }
+        }
+    }
+    
+    pc.printf("Downslopes at: ["); 
+    for(int i = 0; i<numDownslopes; i++){
+        pc.printf("%i ", downslope_locs[i]);
+        
+    }
+    pc.printf("]\n\r");
+    pc.printf("Upslopes at: [");
+    for(int i=0; i<numUpslopes; i++){
+        pc.printf("%i ", upslope_locs[i]);
+    }
+    pc.printf("]\n\r");
+    return track_location;
+}
 
 int main() {
     //Thread thread(linecam_thread);
     //thread.set_priority(osPriorityIdle);
-    pc.printf("Beginning linecam data acquisition... ");
+    pc.printf("Beginning linecam data acquisition... \n\r");
     for(int numFrames =  0; numFrames < maxFrames; numFrames++){
         for(int integrationCounter = 0;integrationCounter < 151;) {
             if(integrationCounter % 151== 0){
@@ -60,7 +120,7 @@
                 clk = 1;
                 wait_us(50);
                 //line_mutex.lock();
-                frames[numFrames][integrationCounter - 1] = camData;
+                frames[numFrames][integrationCounter - 1] = 1-camData; // 1- is for light line
                 //line_mutex.unlock();
                 clk = 0;
             }
@@ -72,10 +132,17 @@
     }
     
     for(int i =0; i<maxFrames; i++){
-        pc.printf("[");
+        pc.printf("LINE %i: [",i);
+        
+        
         for(int j = 0; j < 128; j++){
             pc.printf(" %f",frames[i][j]);
         }
         pc.printf("]\n\r");
+        float* line = frames[i];
+        int track = find_track(line);
+        printf("track at %i\n\r",track);
     }
+    
+    
 }