Diff: main.cpp

Revision:

20:ebdfeb37309c

Parent:

19:25f22034a3e2

Child:

21:18f2dc256df2

--- a/main.cpp	Tue Apr 04 22:50:00 2017 +0000
+++ b/main.cpp	Tue Apr 11 20:25:51 2017 +0000
@@ -5,10 +5,10 @@
 #define FULLLEFT 0.0005
 #define MIN_TURN_RATIO 0
 #define MAX_TURN_RATIO 1
-#define MIN_SPEED 0.15
-#define MAX_SPEED 0.5
+#define MIN_SPEED 0.17  //.15 seems to be optimal
+#define MAX_SPEED 0.45  //.5
 #define TURN_TIME 0
-#define STRAIGHT_TIME 15
+#define STRAIGHT_TIME 5
 #define START_FINISH_TIME 30
 #define DEFAULT_THRESHOLD 65
 #define BLIND_LENGTH 30
@@ -28,6 +28,8 @@
 bool idle = true;
 int leftBlind = 0;
 int rightBlind = 0;
+int numDarks = 0;
+PwmOut led(LED_GREEN);
 
 /*
     Function: setAccel
@@ -35,6 +37,7 @@
                  on the turning angle. 
 */
 void setAccel(float turnAngle){
+    //idle = false;
     if(!idle){
         turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
         float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
@@ -55,7 +58,7 @@
 */
 void turnWheels(int frame[]){
     int positionSum = 0;
-    int numDarks = 0;
+    numDarks = 0;
     for(int i = 0; i < 128; i++){
         if(frame[i] < threshold){
             positionSum += i;
@@ -103,48 +106,52 @@
     Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
 */
 void detectStartFinish(int frame[]){
-    int lookForDark = 1;
+    bool lookForDark = false;
     int darkBlockSize = 0;
     int darkBlocks = 0;
     int lightGap = 0;
     for(int i = 0; i < 128; i++){
         if(lookForDark){
-            if(frame[i] < threshold){
-                darkBlockSize++;
-                if(darkBlockSize > 4){
-                    darkBlocks++;
-                    lookForDark = 0;
+            
+            if(true){
+                if(frame[i] < threshold){
+                    darkBlockSize++;
+                    if(darkBlockSize > 4){ //minimum size for a dark block
+                        darkBlocks++;
+                        lookForDark = false;
+                        darkBlockSize = 0;
+                    }
+                }
+                else if(frame[i] > threshold){
                     darkBlockSize = 0;
                 }
             }
-            else if(frame[i] > threshold){
-                darkBlockSize = 0;
-            }
-        }
-        if(!lookForDark){
-            if(frame[i] > threshold){
-                lightGap++;
-                if(lightGap > 10){ //minimum gap between dark blocks
-                    lookForDark = 1;
-                    lightGap = 0;
+            if(!lookForDark){
+                if(frame[i] > threshold){
+                    lightGap++;
+                }
+                else{
+                    if(lightGap > 20 && lightGap < 55){
+                        lookForDark = true;
+                        lightGap = 0;
+                    }
+                    else{
+                        lightGap = 0;
+                    }
                 }
             }
         }
     }
-    if(startFinishCounter == 0){
         if(darkBlocks > 1){
             idle = !idle; //toggle idle
             startFinishCounter = 1;
+            if(!idle){ 
+                led = 0.0;
+                servo.pulsewidth(STRAIGHT);
+                wait(5);
+            }
+            else led = 1.0;
         }
-    }
-    
-    //don't immediately re-toggle idle
-    if(startFinishCounter > 0 && startFinishCounter < START_FINISH_TIME){
-        startFinishCounter++;
-    }
-    else if(startFinishCounter >= START_FINISH_TIME){
-        startFinishCounter = 0;
-    }
 }
 
 void display(int frame[]){
@@ -156,7 +163,7 @@
         pc.printf("%c", draw);
         draw = 'x';
     }
-    pc.printf("\r\n%d\n\r", idle);
+    pc.printf("\r");
 }
 
 void setThreshold(){
@@ -167,7 +174,7 @@
         if(cam.imageData[i] > high) high = cam.imageData[i];
         if(cam.imageData[i] < low) low = cam.imageData[i];
     }
-    threshold = (high + 2 * low) / 3.5;
+    threshold = low + (high - low) * 0.35;        //(high + 2 * low) / 3;
 }
 
 int main() {    
@@ -177,12 +184,13 @@
     DIR_R = 1;
     DIR_L = 0;
     servo.period(0.020f);
+    led = 1.0;
     while(1){
         wait_ms(5);
         cam.capture();
         //display(cam.imageData);
         turnWheels(cam.imageData);
         setAccel(wheelPos);
-        detectStartFinish(cam.imageData);
+        if(numDarks > 15) detectStartFinish(cam.imageData);
     }
 }
\ No newline at end of file