Diff: main.cpp

Revision:

26:4afa8c5c5156

Parent:

25:74c12b0acf0c

--- a/main.cpp	Tue Apr 18 17:50:55 2017 +0000
+++ b/main.cpp	Thu Apr 20 19:32:16 2017 +0000
@@ -6,8 +6,8 @@
 #define FULLLEFT 0.0005
 #define MIN_TURN_RATIO 0
 #define MAX_TURN_RATIO 1
-#define MIN_SPEED 0.13  //.15 seems to be optimal
-#define MAX_SPEED 0.4  //.5
+#define MIN_SPEED 0.13
+#define MAX_SPEED 0.4 
 #define SPEED_TRIAL_MIN 0.25
 #define SPEED_TRIAL_MAX 0.55
 #define TURN_TIME 0
@@ -16,6 +16,7 @@
 #define DEFAULT_THRESHOLD 65
 #define BLIND_LENGTH 30
 #define DIFF_RATIO 0.5
+#define FINISH_RANGE 30
 
 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
   #define ELEC0 9
@@ -44,15 +45,19 @@
 int leftBlind = 0;
 int rightBlind = 0;
 float lastSlide;
+int positionSum = 0;
 int numDarks = 0;
 int minLightGap = 1;
 int maxLightGap = 55;
 int minDarkBlock = 4;
 int maxDarkBlock = 40;
+float averagePos = 0;
 int positionOffThreshold = 3;
 struct darkBlock *darkBlockHead;
+int noBlocks;
 PwmOut led(LED_GREEN);
 PwmOut redLed(LED_RED);
+bool finished = false;
 
 /*
     Function: setAccel
@@ -80,15 +85,7 @@
                  by the camera
 */
 void turnWheels(int frame[]){
-    int positionSum = 0;
-    numDarks = 0;
-    for(int i = 0; i < 128; i++){
-        if(frame[i] < threshold){
-            positionSum += i;
-            numDarks++;
-        }
-    }
-    float averagePos = 0;
+    averagePos = 0;
 
         if (numDarks == 0) {
              if(turnCounter >= (STRAIGHT_TIME)){
@@ -124,6 +121,9 @@
     servo.pulsewidth(wheelPos);
 }
 
+/*
+    deprecated
+*/
 struct darkBlock{
     int position;
     int length;
@@ -136,31 +136,49 @@
     Function: detectStartFinish
     Description: detects the mark on the track that represents the start/finish line and toggles RUNNING
 */
-void detectStartFinish(int frame[]){
-    
+void detectStartFinish(){
+
     //idle override by touching the slider
     if(tsi.readPercentage() != lastSlide){
         idle = !idle;
-        led = 0.0;
+        led = 1.0 - led;
         turn_speed = MIN_SPEED + (SPEED_TRIAL_MIN - MIN_SPEED) *  tsi.readPercentage();
         straight_speed = MAX_SPEED + (SPEED_TRIAL_MAX - MAX_SPEED) *  tsi.readPercentage();
         wait(0.5);
-        
-    }
-    return;
-    if(numDarks <= 15) return;
-    for(int i = 0; i < 128; i++){
-        
     }
     
-    idle = !idle; //toggle idle
-        if(!idle){ 
-            led = 1.0 - led;
-            servo.pulsewidth(STRAIGHT);
-            wait(3);
+    if(numDarks > 18 && averagePos > 63 - FINISH_RANGE && averagePos < 63 + FINISH_RANGE && noBlocks <= 2 && (!finished || !idle)){
+        led = 1.0 - led;
+        idle = !idle; //toggle idle
+            if(!idle){ 
+                servo.pulsewidth(STRAIGHT);
+                wait(2);
+            }
+            else finished = true;
+    }
+}
+/*
+    Function: processInput
+    Description: This function processes the input gathered from the camera, and calculates variables used by other parts of the car
+    
+*/
+void processInput(int frame[]){
+        positionSum = 0;
+    numDarks = 0;
+    bool flag = true;
+    noBlocks = 0;
+    for(int i = 0; i < 128; i++){
+        if(frame[i] < threshold){
+            positionSum += i;
+            numDarks++;
+            if(flag) {
+                flag = false;
+                noBlocks++;
+            }
         }
-        else led = 1.0;
-}
+        else flag = true;
+    }
+    }
 
 /*
     Function: display
@@ -181,7 +199,7 @@
         pc.printf("%c", draw);
         draw = 'x';
     }
-    pc.printf("\r");
+    pc.printf(" darks %d av %d blocks %d\r", numDarks, averagePos, noBlocks);
 }
 
 /*
@@ -215,9 +233,10 @@
     while(1){
         wait_ms(1);
         cam.capture();
-        //display(cam.imageData);
+        processInput(cam.imageData);
+        detectStartFinish();
         turnWheels(cam.imageData);
+        //display(cam.imageData);
         setAccel(wheelPos);
-        detectStartFinish(cam.imageData);
     }
 }
\ No newline at end of file