diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.cpp
index 1b0f09435d292ce47b4ba407425275d988c22794..fb27cf808991a0b2083ca36f412ba1aa088527e6 100644
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.cpp	
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.cpp	
@@ -21,33 +21,18 @@ void Camera::exe_face_detection(){
         Serial.println(F("]"));
     }
 }
-  
-void Camera::exe_person_detection(){
-    char buff[32 + 1] = {}; // null terminator
-    if (interface->call_no_args(F("person_detection"), buff, sizeof(buff) - 1)) {
-        Serial.println(buff);
-    }
-}
-
-void Camera::exe_qrcode_detection(){
-    char buff[128 + 1] = {}; // null terminator
-    if (interface->call_no_args(F("qrcode_detection"), buff, sizeof(buff) - 1)) {
-        Serial.print("qrdetected: ");
-        Serial.println(buff);
-    }
-}
 
-void Camera::exe_datamatrix_detection(){
-    char buff[128 + 1] = {}; // null terminator
-    if (interface->call_no_args(F("datamatrix_detection"), buff, sizeof(buff) - 1)) {
-        Serial.println(buff);
+bool Camera::exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp){
+    struct { uint16_t cx, cy, rot; } result;
+    if (interface->call(F("apriltags"), &ID, sizeof(ID), &result, sizeof(result))) {
     }
-}
-
-void Camera::exe_barcode_detection(){
-    char buff[128 + 1] = {}; // null terminator
-    if (interface->call_no_args(F("barcode_detection"), buff, sizeof(buff) - 1)) {
-        Serial.println(buff);
+    if (result.cx == 0 && result.cy == 0 && result.rot == 0){
+      return false;
+    } else {
+      *x_temp = result.cx;
+      *y_temp = result.cy;
+      *angle_temp = result.rot;
+      return true;
     }
 }
   
@@ -84,52 +69,23 @@ bool Camera::exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t
   }
 }
 
-void Camera::exe_led_detection(){
-      //int8_t color_thresholds[6] = {l_min, l_max, a_min, a_max, b_min, b_max};
-    int8_t color_thresholds[6] = {26, 100, -108, -9, 0, -42};
-    //struct { uint16_t cx, cy; } color_detection_result;
-    short buff[128 + 1] = {};
-    if (interface->call(F("LED_detection"), color_thresholds, sizeof(color_thresholds), buff, sizeof(buff)-1)) {
-          int i = 0;
-          Serial.println("LED detected");
-          while (buff[i] != '\0' && i<100) {
-            Serial.print(buff[i]);
-            i++;  
-          }
-          Serial.println("");
-    }
-}
-
-bool Camera::exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp){
-    struct { uint16_t cx, cy, rot; } result;
-    if (interface->call(F("apriltags"), &ID, sizeof(ID), &result, sizeof(result))) {
-    }
-    if (result.cx == 0 && result.cy == 0 && result.rot == 0){
-      return false;
-    } else {
-      *x_temp = result.cx;
-      *y_temp = result.cy;
-      *angle_temp = result.rot;
-      return true;
-    }
-}
-
 
-bool Camera::exe_goalfinder(int goal1, int goal2, int goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz){
-  int goalid[3] = {goal1, goal2, goal3};
+bool Camera::exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz){
+  int8_t goalid[3] = {goal1, goal2, goal3};
   struct { uint16_t cid, ctx, cty, ctz, crx, cry, crz; } goalfinder_result;
-  interface->call(F("goalfinder"), goalid, sizeof(goalid), goalfinder_result, sizeof(goalfinder_result));
- 
+  if(interface->call(F("goalfinder"), goalid, sizeof(goalid), &goalfinder_result, sizeof(goalfinder_result))){
+    
+  }
   if (goalfinder_result.crx == 0 && goalfinder_result.cry == 0){
     return false;
   } else {
-    goalfinder.cid = id;
-    goalfinder.ctx = tx;
-    goalfinder.cty = ty;
-    goalfinder.ctz = tz;
-    goalfinder.crx = rx;
-    goalfinder.cry = ry;
-    goalfinder.crz = rz;
+    id = goalfinder_result.cid;
+    tx = goalfinder_result.ctx;
+    ty = goalfinder_result.cty;
+    tz = goalfinder_result.ctz;
+    rx = goalfinder_result.crx;
+    ry = goalfinder_result.cry;
+    rz = goalfinder_result.crz;
     return true;
   }
 }
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.h b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.h
index 95ac916fa132fb06297c5b0f005c0f929a389aec..5a4536e6d0967e96fd5864e596b8f8f9f0312289 100644
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.h	
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_April_Tag/Camera.h	
@@ -13,8 +13,7 @@ class Camera
   bool exe_apriltag_detection(int ID,int *x_temp,int *y_temp,int *angle_temp);
   bool exe_color_detection(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max);
   bool exe_color_detection_biggestblob(int8_t l_min, int8_t l_max, int8_t a_min, int8_t a_max, int8_t b_min, int8_t b_max, int& x, int&y);
-  void exe_led_detection();
-  bool exe_goalfinder(int goal1, int goal2, int goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz); //optional to add tag size as parameter?
+  bool exe_goalfinder(int8_t goal1, int8_t goal2, int8_t goal3, int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz); //optional to add tag size as parameter?
 
 };
 #endif