added control strategy as if-else's

Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino

@@ -29,6 +29,7 @@ int pau = 0;
 int displayTracking = 0;
 int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
 int BASE_SPEED = 50; //125;
+int ball_capture = 0;
 
 //The debug LEDs that we will be using. Description at:
 const int DEBUG_STATE = 31;
@@ -96,21 +97,9 @@ void loop() {
   int xtemp = 0;
   int ytemp = 0;
   int angletemp = 0;
-//  if(cam.exe_apriltag_detection(ENDDEMO_TAG, &xtemp, &ytemp, &angletemp)){
-//    int zero = 0;
-//    moveVertical(zero);
-//    moveHorizontal(zero,zero);
-//    panel.singleLED(DEBUG_STATE, 0, 0, 10);
-//    while(1){
-//      Serial.println("end of demo");
-//    }
-//  }
-
-  //if the demo is still ongoing, check to see if there is a desired-color blob
   int x = 0;
   int y = 0;
   panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
-  //if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
   int id = -1;
   int tx = 0;
   int ty = 0;
@@ -119,41 +108,58 @@ void loop() {
   int ry = 0;
   int rz = 0;
   int8_t goal[3] = {0,1,2};
-  if(cam.exe_goalfinder(goal[0], goal[1], goal[2], id, x, y, tz, rx, ry, rz)){
-    if (displayTracking > 0){
-      displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
+  
+  ball_capture = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)
+  
+  if(ball_capture){
+	if(cam.exe_apriltag_detection(ENDDEMO_TAG, &xtemp, &ytemp, &angletemp)){
+      int zero = 0;
+      moveVertical(zero);
+      moveHorizontal(zero,zero);
+      panel.singleLED(DEBUG_STATE, 0, 0, 10);
+      while(1){
+        Serial.println("end of demo");
+      }
     }
-    panel.singleLED(DEBUG_STATE, 0, 10, 0);
-    Serial.println("blob detected");
-    Serial.print("x value: ");
-    Serial.println(x);
-    Serial.print("y value: ");
-    Serial.println(y);
-    Inputx = x/1.00;
-    Inputy = y/1.00;
-    PID_x.Compute();
-    PID_y.Compute();
-
-    Serial.println(Outputy);
-    Serial.println(Outputx);
-    Serial.print("Outputx: ");
-    Serial.println(Outputx);
-    //actuate the vertical motor
-    moveVertical(Outputy);
-    moveHorizontal(Outputx, BASE_SPEED);
+	if(cam.exe_goalfinder(goal[0], goal[1], goal[2], id, x, y, tz, rx, ry, rz)){
+      if (displayTracking > 0){
+		displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK
+      }
+      panel.singleLED(DEBUG_STATE, 0, 10, 0);
+      Serial.println("blob detected");
+      Serial.print("x value: ");
+      Serial.println(x);
+      Serial.print("y value: ");
+      Serial.println(y);
+      Inputx = x/1.00;
+      Inputy = y/1.00;
+      PID_x.Compute();
+      PID_y.Compute();
+  
+      Serial.print("Outputx: ");
+      Serial.println(Outputx);
+      Serial.print("Outputy: ");
+      Serial.println(Outputy);
+      
+	  moveVertical(Outputy);
+      moveHorizontal(Outputx, BASE_SPEED);
     
-  } else { //seeking algorithm
-    //panel.resetPanel();
-    panel.singleLED(DEBUG_STATE, 10, 10, 10);
-    Serial.println("seeking...");
-    int zero = 0;
-    moveVertical(zero);
-    moveHorizontal(SEEKING_SPEED, zero);
+    } else { //seeking algorithm
+      //panel.resetPanel();
+      panel.singleLED(DEBUG_STATE, 10, 10, 10);
+      Serial.println("seeking...");
+      int zero = 0;
+      moveVertical(zero);
+      moveHorizontal(SEEKING_SPEED, zero);
+    }
+  }
+  else {
+	// seek the ball
+	
   }
 }
 
-//vel value should be between -255 to 255 with positive values moving the blimp
-//upward.
+//vel value should be between -255 to 255 with positive values moving the blimp upward.
 void moveVertical(int vel){
     if (vel > 0) { //up
       panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0);