Diff: main.cpp

Revision:

19:25f22034a3e2

Parent:

18:8dbd05e65211

Child:

20:ebdfeb37309c

--- a/main.cpp	Sun Apr 02 23:04:56 2017 +0000
+++ b/main.cpp	Tue Apr 04 22:50:00 2017 +0000
@@ -9,6 +9,7 @@
 #define MAX_SPEED 0.5
 #define TURN_TIME 0
 #define STRAIGHT_TIME 15
+#define START_FINISH_TIME 30
 #define DEFAULT_THRESHOLD 65
 #define BLIND_LENGTH 30
 #define DIFF_RATIO 0.5
@@ -21,9 +22,10 @@
 Serial pc(USBTX, USBRX);
 Camera cam(PTE23, PTE21, PTB3);
 int turnCounter = 0;
+int startFinishCounter = 0;
 int threshold = DEFAULT_THRESHOLD;
 float wheelPos = STRAIGHT;
-bool idle = false;
+bool idle = true;
 int leftBlind = 0;
 int rightBlind = 0;
 
@@ -32,12 +34,18 @@
     Description: Sets the speed for the right and left motors individually based
                  on the turning angle. 
 */
-void setAccel(float turnAngle){//, float speed){
-    turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
-    float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
-    float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
-    motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
-    motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
+void setAccel(float turnAngle){
+    if(!idle){
+        turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
+        float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
+        float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
+        motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
+        motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
+    }
+    else{
+        motor_left.write(0);
+        motor_right.write(0);
+    }
 }//end setAccel
 
 /*
@@ -90,6 +98,55 @@
     servo.pulsewidth(wheelPos);
 }
 
+/*
+    Function: detectStartFinish
+    Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
+*/
+void detectStartFinish(int frame[]){
+    int lookForDark = 1;
+    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;
+                    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(startFinishCounter == 0){
+        if(darkBlocks > 1){
+            idle = !idle; //toggle idle
+            startFinishCounter = 1;
+        }
+    }
+    
+    //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[]){
     char draw = 'x';
     for(int i = 0; i< 128; i++){
@@ -97,10 +154,9 @@
         if (frame[i] < threshold) draw = '|';
         else draw = '-';
         pc.printf("%c", draw);
-        //pc.printf("%d", frame[i]);
         draw = 'x';
     }
-    pc.printf("\r");
+    pc.printf("\r\n%d\n\r", idle);
 }
 
 void setThreshold(){
@@ -111,7 +167,7 @@
         if(cam.imageData[i] > high) high = cam.imageData[i];
         if(cam.imageData[i] < low) low = cam.imageData[i];
     }
-    threshold = (high + 2 * low) / 3;
+    threshold = (high + 2 * low) / 3.5;
 }
 
 int main() {    
@@ -125,7 +181,8 @@
         wait_ms(5);
         cam.capture();
         //display(cam.imageData);
-        turnWheels(cam.imageData);      
-        if(!idle) setAccel(wheelPos);
+        turnWheels(cam.imageData);
+        setAccel(wheelPos);
+        detectStartFinish(cam.imageData);
     }
 }
\ No newline at end of file