Diff: main.cpp

Revision:

1:55e0aaf71bda

Parent:

0:ad375c052b4c

Child:

2:a04e2757d372

--- a/main.cpp	Fri Mar 20 08:59:05 2015 +0000
+++ b/main.cpp	Fri Mar 20 10:18:20 2015 +0000
@@ -79,6 +79,59 @@
 
 //End of Encoder and Motor Driver Variables ----------------------
 
+//Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+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 ;
+            }
+        }
+    }
+
+    return track_location;
+}
+
+
 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 float get_speed(){
@@ -169,19 +222,19 @@
     // Motor Driver Initializations
     motor1.period(.0025);
     motor2.period(.0025);
-    interrupt.fall(&fallInterrupt);
-    interrupt.rise(&riseInterrupt);
+    //interrupt.fall(&fallInterrupt);
+    //interrupt.rise(&riseInterrupt);
 
-    //wait(5);
+    wait(5);
 
-    motor1.pulsewidth(.0025*.1);
-    motor2.pulsewidth(.0025*.1);
+    motor1.pulsewidth(.0025*.105);
+    motor2.pulsewidth(.0025*.105);
     break1 = 0;
     break2 = 0;
     
-    t.start();
+    //t.start();
     
-    wait(5);
+    //wait(5);
     
     while(1) {
             
@@ -226,15 +279,15 @@
             //interrupt.rise(&riseInterrupt);
             
             minVal = ADCdata[15];
-            for (int c = 15; c < 118; c++) {
+            for (int c = 10; c < 118; c++) {
                 if (ADCdata[c] < minVal){
                     minVal = ADCdata[c];
                     minLoc = c;
                 }
             }
             
-            for (int c = 15; c < 118; c++) {
-                if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.04f && ADCdata[c] > 0.1f){
+            for (int c = 10; c < 118; c++) {
+                if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.08f && ADCdata[c] > 0.1f){
                     minAccum += c;
                     minCount++;
                 }
@@ -242,26 +295,46 @@
             
             approxPos = (float)minAccum/(float)minCount;
             
-            if(approxPos > 0 && approxPos <= 20){
+            
+            //approxPos = find_track(ADCdata);
+            
+            if(approxPos > 0 && approxPos <= 25){
                     servo.pulsewidth(hardLeft);
-            } else if (approxPos > 20 && approxPos <= 45){
+                    motor1.pulsewidth(.0025*.095);
+                    motor2.pulsewidth(.0025*.095);
+            } else if (approxPos > 25 && approxPos <= 40){
                     servo.pulsewidth(slightLeft);
-            } else if (approxPos > 45 && approxPos <= 90){
+                    motor1.pulsewidth(.0025*.1);
+                    motor2.pulsewidth(.0025*.1);
+            } else if (approxPos > 40 && approxPos <= 90){
                 servo.pulsewidth(straight);
+                    motor1.pulsewidth(.0025*.105);
+                    motor2.pulsewidth(.0025*.105);
             } else if (approxPos > 90 && approxPos <= 105){
-                servo.pulsewidth(slightRight);
+                    servo.pulsewidth(slightRight);
+                    motor1.pulsewidth(.0025*.1);
+                    motor2.pulsewidth(.0025*.1);
             } else if (approxPos > 105 && approxPos <= 128){
                     servo.pulsewidth(hardRight);
+                    motor1.pulsewidth(.0025*.095);
+                    motor2.pulsewidth(.0025*.095);
             }
-            
-            velocity_control(0.05f, DESIRED_SPEED);
+            /*
+            if(approxPos > 0 && approxPos <= 45){
+                    servo.pulsewidth(hardLeft);
+            } else if (approxPos > 45 && approxPos <= 95){
+                    servo.pulsewidth(straight);
+            } else {
+                servo.pulsewidth(hardRight);
+            }*/
+            //velocity_control(0.05f, DESIRED_SPEED);
             
             integrationCounter = 150;
         }
         else{
             clk = 1;
-            wait_us(70);
-            ADCdata[integrationCounter - 1] = camData;
+            wait_us(220);
+            ADCdata[integrationCounter - 1] = 1 - camData;
             clk = 0;
         }