diff --git a/Code/feather_main_personaldemo.ino b/Code/feather_main_personaldemo.ino
new file mode 100644
index 0000000000000000000000000000000000000000..84fe42156ef837399c01dbfd052c27b995aafc2d
--- /dev/null
+++ b/Code/feather_main_personaldemo.ino
@@ -0,0 +1,201 @@
+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+#include "Adafruit_VL53L0X.h"
+#include "ArnholdMesh.h"
+#include <Arduino.h>
+#include <string>
+#include <vector>
+#include "Camera.h"
+#include <PID_v1.h>
+
+#define STATION_SSID "lemur"
+#define STATION_PASSWORD ""
+#define DDRIVE_MIN -1 //The minimum value x or y can be.
+#define DDRIVE_MAX 1 //The maximum value x or y can be.
+#define MOTOR_MIN_PWM -255 //The minimum value the motor output can be.
+#define MOTOR_MAX_PWM 255 //The maximum value the motor output can be.
+double Setpointx, Inputx, Outputx;
+double Setpointy, Inputy, Outputy;
+
+//Identify all the global constants that will be used by the robot
+const int BAUD_RATE = 115200;
+const int MAX_SPEED = 255;
+const double RESOLUTION_W = 320.0;
+const double RESOLUTION_H = 240.0;
+const int SEEKING_SPEED = 60;
+const int TIME_STEP = 1;
+const int ENDDEMO_TAG = 0;
+
+//Identify all the variables that will be used by the robot
+int findcolor = 1; //based on the same code from openMV. Determine the color you want to find
+int NOMINAL_PERCENTAGE = 80; //in percent
+char newPTCL = '1';
+int altitude = 0;
+int desiredAltitude = 0;
+int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
+
+////Identify the variables that will be communicated through DASH
+//bool manualMode = true;
+//int verticalSpeed = 0; //0 to 255
+//bool verticalDirection = true;
+//float horiAnalog = 0; //between -1 to 1
+//float vertAnalog = 0;
+
+//Create the interface that will be used by the camera
+openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
+openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
+
+// Create the motor shield object with the default I2C address
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+Adafruit_DCMotor *motorVertical = AFMS.getMotor(1);
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(2);
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
+
+//Create the LIDAR object
+//Adafruit_VL53L0X lidar = Adafruit_VL53L0X();
+
+//ArnholdMesh thisNode;
+Camera cam(&interface);
+
+//Specify the links and initial tuning parameters
+double Kp=1, Ki=0.05, Kd=0.25;
+PID PID_x(&Inputx, &Outputx, &Setpointx, Kp, Ki, Kd, DIRECT);
+PID PID_y(&Inputy, &Outputy, &Setpointy, Kp, Ki, Kd, DIRECT);
+
+void setup() {
+ Serial.begin(BAUD_RATE);
+ translateCodetoThreshold(findcolor);
+ Wire.begin();
+ AFMS.begin(); // create with the default frequency 1.6KHz
+ interface.begin();
+
+ Setpointx = 160.0;
+ Setpointy = 120.0;
+ PID_y.SetOutputLimits(-255, 255);
+ PID_x.SetOutputLimits(-255, 255);
+ PID_x.SetMode(AUTOMATIC);
+ PID_y.SetMode(AUTOMATIC);
+
+ //thisNode.setStationInfo(STATION_SSID, STATION_PASSWORD);
+ //thisNode.init();
+// if (!lidar.begin()) {
+// while(1){
+// //NEED: Light up red LED
+// }
+// }
+}
+
+void loop() {
+ int x = 0;
+ int y = 0;
+ if(cam.exe_color_detection_biggestblob(threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], threshold[6], x, y)){
+ Serial.print("x value: ");
+ Serial.println(x);
+ Serial.print("y value: ");
+ Serial.println(y);
+ Inputx = x/1.00;
+ Inputy = y/1.00;
+ PID_x.Compute();
+ PID_y.Compute();
+ Serial.print("the x output is: ");
+ Serial.println(Outputx);
+ Serial.print("the y output is: ");
+ Serial.println(Outputy);
+ } else {
+ Serial.println("not detected");
+ }
+
+// //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)){
+// motorVertical->setSpeed(0);
+// motorVertical->run(FORWARD);
+// // turn on motor
+// motorVertical->run(RELEASE);
+// motorLeft->setSpeed(0);
+// motorLeft->run(FORWARD);
+// motorRight->setSpeed(0);
+// motorRight->run(BACKWARD);
+// while(1){
+// Serial.println("end of demo");
+// }
+// }
+//
+// int x = 0;
+// int y = 0;
+// if(cam.exe_color_detection_biggestblob(threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], threshold[6], x, y)){
+// Serial.println("blob detected");
+// Input = x;
+// myPID.Compute();
+// Serial.print("the output is: ");
+// Serial.println(Output);
+// int dy = y - RESOLUTION_H/2.0;
+// int dx = x - RESOLUTION_W/2.0;
+// int y_speed = abs(dy/(RESOLUTION_H/2.0))*MAX_SPEED*NOMINAL_PERCENTAGE/100.0;
+// int x_speed = abs(dx/(RESOLUTION_W/2.0))*MAX_SPEED*NOMINAL_PERCENTAGE/100.0;
+// int lockon_speed = MAX_SPEED*NOMINAL_PERCENTAGE/200.0;
+// Serial.print("y value: ");
+// Serial.println(y);
+// Serial.print("velocity: ");
+// Serial.println(y_speed);
+// if (dy < 0) {
+// motorVertical->setSpeed(100 + y_speed);
+// motorVertical->run(BACKWARD);
+// } else {
+// motorVertical->setSpeed(100 + y_speed);
+// motorVertical->run(FORWARD);
+// }
+// if (dx < 0) { //
+// motorRight->setSpeed(SEEKING_SPEED/2); //(SEEKING_SPEED);
+// motorRight->run(BACKWARD);
+// motorLeft->setSpeed(min(255, (SEEKING_SPEED/2) + x_speed)); //this need to be higher
+// motorLeft->run(BACKWARD);
+// } else {
+// motorRight->setSpeed(min(255,(SEEKING_SPEED/2) + x_speed)); //this need to be higher
+// motorRight->run(BACKWARD);
+// motorLeft->setSpeed(SEEKING_SPEED/2); //(SEEKING_SPEED);
+// motorLeft->run(BACKWARD);
+// }
+// } else { //seeking algorithm
+// Serial.println("seeking...");
+// motorVertical->setSpeed(0);
+// motorVertical->run(FORWARD);
+// // turn on motor
+// motorVertical->run(RELEASE);
+// Serial.println(SEEKING_SPEED*NOMINAL_PERCENTAGE/100.0);
+// motorLeft->setSpeed(SEEKING_SPEED*NOMINAL_PERCENTAGE/100.0);
+// motorLeft->run(FORWARD);
+// motorRight->setSpeed(SEEKING_SPEED*NOMINAL_PERCENTAGE/100.0);
+// motorRight->run(BACKWARD);
+// }
+}
+
+void translateCodetoThreshold(int code){
+ switch(code){
+ case 1: //green
+ threshold[1] = 30;
+ threshold[2] = 100;
+ threshold[3] = -68;
+ threshold[4] = -13;
+ threshold[5] = 30;
+ threshold[6] = 127;
+ break;
+ case 2: //blue
+ threshold[1] = 30;
+ threshold[2] = 100;
+ threshold[3] = -108;
+ threshold[4] = -9;
+ threshold[5] = 0;
+ threshold[6] = -42;
+ break;
+ case 5: //red
+ threshold[1] = 30;
+ threshold[2] = 100;
+ threshold[3] = 127;
+ threshold[4] = 23;
+ threshold[5] = 127;
+ threshold[6] = -13;
+ }
+}