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 )));