diff --git a/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino b/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino index 87cd60257913ac61c69853e4aa76274ab439c0a0..2e12cb8e8e3cbc7df0f6b850aa4ef4ea92027338 100644 --- a/Code/Control/Feather Code - Aaron/PID_Test/PID_Test.ino +++ b/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);