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 19871317e90208a9c72cdb0ca24077274a842db1..ae4dc1b6f3a06ae79dd6ac6540babe7d6b1a7417 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
@@ -31,7 +31,7 @@ char newPTCL = '1';
 int pau = 0;
 int displayTracking = 0;
 int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 100; //125;
+int BASE_SPEED = 70; //125;
 
 //The debug LEDs that we will be using. Description at:
 const int DEBUG_STATE = 31;
@@ -52,8 +52,8 @@ openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, cons
 // Create the motor shield object with the default I2C address
 Adafruit_MotorShield AFMS = Adafruit_MotorShield();
 // changed
-Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(2); 
-Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(1);
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1); 
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);
 Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
 Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
 // changed
@@ -106,51 +106,30 @@ int goal_id[3] = {0, 1, 2};
 
 void loop() {  
   panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
-  
-  //Check to see if we want to end the demo
-  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
-  
-  int goal_id[3] = {0, 1, 2};
-  int id = 0; 
+  int id = -1;
   int tx = 0; int ty = 0; int tz = 0;
   int rx = 0; int ry = 0; int rz = 0;
+  int8_t goal[3] = {0,1,2};
   
-  # 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
-    }
+  if(cam.exe_goalfinder(goal[0],goal[1],goal[2], id, tx, ty, tz, rx, ry, rz)){
     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.print("x: ");
     Serial.println(Outputx);
-    
+        Serial.print("y: ");
+    Serial.println(Outputy);
     moveVertical(Outputy);
     moveHorizontal(Outputx, BASE_SPEED);
   }else { // seeking
     Serial.println("seeking...");
-    moveVertical(zero);
+    moveVertical(0);
     moveHorizontal(SEEKING_SPEED, 0);
     }
   
@@ -192,15 +171,17 @@ void moveVertical(int vel){
       motorVertical_L->setSpeed(abs((int) vel));
       motorVertical_R->setSpeed(abs((int) vel)); 
       motorVertical_L->run(BACKWARD);
-      motorVertical_R->run(BACKWARD);
+      motorVertical_R->run(FORWARD);
       // 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(FORWARD);
-      motorVertical_R->run(FORWARD);
+      motorVertical_L->setSpeed(abs((int) vel));  
+      motorVertical_R->setSpeed(abs((int) vel)); 
+            motorVertical_L->run(FORWARD);
+      motorVertical_R->run(BACKWARD);
+
+
       // changed
     } else {
       panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
@@ -217,19 +198,18 @@ void moveHorizontal(int vel_hori,int base_speed){
   int rspeed = vel_hori + base_speed;
 
   if (lspeed > 0){  
-    motorRight->run(FORWARD);
+    motorLeft->run(BACKWARD);
   } else {
-    motorRight->run(BACKWARD);
+    motorLeft->run(FORWARD);
   }
-
   if (rspeed > 0){
-    motorLeft-> run(BACKWARD);
+    motorRight-> run(FORWARD);
   } else {
-    motorLeft-> run(FORWARD);
+    motorRight-> run(BACKWARD);
   }
   displaySpeed(lspeed, rspeed);
-  motorLeft->setSpeed(min(MAX_SPEED, abs(rspeed )));
-  motorRight->setSpeed(min(MAX_SPEED, abs(lspeed)));
+  motorLeft->setSpeed(min(MAX_SPEED, abs(lspeed )));
+  motorRight->setSpeed(min(MAX_SPEED, abs(rspeed)));
 }
 
 void displaySpeed(int lspeed, int rspeed){