diff --git a/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino b/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino
index 089e84a4650976502bcf6a28e3925d89fd9bf702..1e734168a85936cada01956ed9db020a247cf5ec 100644
--- a/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino	
+++ b/Code/Kamil's Feather Code/feather_main_personaldemo_PID.ino	
@@ -1,5 +1,6 @@
 #include <Wire.h>
 #include <Adafruit_MotorShield.h>
+//#include "Adafruit_VL53L0X.h"
 #include "ArnholdMesh.h"
 #include "LedPanel.h"
 #include <Arduino.h>
@@ -33,7 +34,7 @@ char newPTCL = '1';
 int pau = 0;
 int displayTracking = 0;
 int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
-int BASE_SPEED = 20; //125;
+int BASE_SPEED = 50; //125;
 
 //The debug LEDs that we will be using. Description at:
 const int DEBUG_STATE = 31;
@@ -118,27 +119,30 @@ 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(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
-//  Serial.print("current threshold: ");
-//    for (int j = 0; j < 6; j++){
-//    Serial.print(threshold[j]);
-//    Serial.print(" ");
-//  }
-//  Serial.println("");
-  if(cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)){
+  //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;
+  int tz = 0;
+  int rx = 0;
+  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
     }
@@ -412,6 +416,25 @@ void debugPIDConstants(int lednum, double oldval, double newval){
 
 
 
+//int goal_id[3] = {0, 1, 2};
+//////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; 
+//  if(cam.exe_goalfinder(goal_id[0], goal_id[1], goal_id[2], int&id, int&tx, int&ty, int&tz, int&rx, int&ry, int&rz)){
+//    //apply the same PID control when seeking green ball
+//    //if we are at a specific position near the goal (slightly above), move straight forward for 5 second
+//  } else {
+//    //seeking mechanism to find the goal
+//    //move up until we reach the desired altitude
+//    //if already at the desired altitude, rotate around  
+//  } 
+//} else { //no ball in basket
+//  //do whatever we have been doing before to find a green balloon
+//}
+
+
+
 
 ////MESH COMMUNICATION UTILITY FUNCTIONS////
 void interpretDashInstructions(String& msg){