diff --git a/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino b/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino index 4ebadbd21ace67ea788bd759b02043587b831567..19871317e90208a9c72cdb0ca24077274a842db1 100644 --- a/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino +++ b/Code/Feather_Code_Test/feather_main_test_PID_April_Tag/feather_main_test_PID_April_Tag.ino @@ -88,7 +88,7 @@ void setup() { // ==============================main loop==================================== int goal_id[3] = {0, 1, 2}; -//////goal seeking algorithm +////goal seeking algorithm //if (lidar detect green ball){ // int id = -1; int tx = 0; int ty = 0; int tz = 0; // int rx = 0; int ry = 0; int rz = 0; @@ -106,13 +106,6 @@ int goal_id[3] = {0, 1, 2}; void loop() { panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED); - while(pau == 1){ - int zero = 0; - moveVertical(zero); - moveHorizontal(zero,zero); - panel.singleLED(DEBUG_STATE, 10, 10, 0); - Serial.println("pause"); - } //Check to see if we want to end the demo int xtemp = 0; @@ -133,6 +126,35 @@ void loop() { int x = 0; int y = 0; panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby + + int goal_id[3] = {0, 1, 2}; + int id = 0; + int tx = 0; int ty = 0; int tz = 0; + int rx = 0; int ry = 0; int rz = 0; + + # if(exe_goalfinder(int goal_id[0], int goal_id[1],int goal_id[2], int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz)) + if(exe_goalfinder(goal_id[0],goal_id[1],goal_id[2],&id,&tx, &ty, &tz, &rx, &ry, &rz)){ + if (displayTracking > 0){ + displayTrackedObject(tx, ty, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK + } + panel.singleLED(DEBUG_STATE, 0, 10, 0); + Inputx = tx/1.00; + Inputy = ty/1.00; + PID_x.Compute(); + PID_y.Compute(); + + Serial.println(Outputy); + Serial.println(Outputx); + + moveVertical(Outputy); + moveHorizontal(Outputx, BASE_SPEED); + }else { // seeking + Serial.println("seeking..."); + moveVertical(zero); + moveHorizontal(SEEKING_SPEED, 0); + } + + /* if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){ if (displayTracking > 0){ displayTrackedObject(x, y, RESOLUTION_W, RESOLUTION_H); //THIS NEEDS WORK @@ -155,6 +177,7 @@ void loop() { moveVertical(zero); moveHorizontal(SEEKING_SPEED, zero); } + */ } // ==============================main loop==================================== @@ -168,16 +191,16 @@ void moveVertical(int vel){ // changed motorVertical_L->setSpeed(abs((int) vel)); motorVertical_R->setSpeed(abs((int) vel)); - motorVertical_L->run(FORWARD); - motorVertical_R->run(FORWARD); + motorVertical_L->run(BACKWARD); + motorVertical_R->run(BACKWARD); // changed } else if(vel < 0) { //down panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel)); // changed motorVertical_L->setSpeed(abs((int) Outputy)); motorVertical_R->setSpeed(abs((int) Outputy)); - motorVertical_L->run(BACKWARD); - motorVertical_R->run(BACKWARD); + motorVertical_L->run(FORWARD); + motorVertical_R->run(FORWARD); // changed } else { panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0); @@ -194,15 +217,15 @@ void moveHorizontal(int vel_hori,int base_speed){ int rspeed = vel_hori + base_speed; if (lspeed > 0){ - motorLeft->run(BACKWARD); + motorRight->run(FORWARD); } else { - motorLeft->run(FORWARD); + motorRight->run(BACKWARD); } if (rspeed > 0){ - motorRight->run(FORWARD); + motorLeft-> run(BACKWARD); } else { - motorRight->run(BACKWARD); + motorLeft-> run(FORWARD); } displaySpeed(lspeed, rspeed); motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed )));