diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Localization.cpp b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Localization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/Localization.cpp	
@@ -0,0 +1 @@
+
diff --git a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino
index 3a05f2fb213094bdc33aad8d511202516bb3e49e..a619a0782e96e7db042e26e10eb301e6b99e72cf 100644
--- a/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino	
+++ b/Code/Control/Feather Code - Zhaoliang/feather_main_test_PID_Lidar_Full/feather_main_test_PID_Lidar_Full.ino	
@@ -1,8 +1,7 @@
-
 #include <SparkFun_VL53L1X.h>
 #include <Wire.h>
 #include <Adafruit_MotorShield.h>
-
+#include <Adafruit_MPL3115A2.h>
 #include "LedPanel.h"
 #include <Arduino.h>
 #include <string>
@@ -18,7 +17,13 @@ SFEVL53L1X distanceSensor;
 int budgetIndex = 4;
 int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
 int LED = LED_BUILTIN;
-// 
+
+float groundLevel = 120;
+float lowHeightBarrier  = 4;
+float highHeightBarrier = 4;
+float ceilingHeight = 20;
+
+Adafruit_MPL3115A2 baro;
 
 //Identify all the global constants that will be used by the robot
 const int BAUD_RATE = 115200;
@@ -37,6 +42,8 @@ int displayTracking = 0;
 int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
 int BASE_SPEED = 70; //125;
 
+int hoverAltitude = -1;
+
 //The debug LEDs that we will be using. Description at:
 const int DEBUG_STATE = 31;
 const int DEBUG_KP = 30;
@@ -86,6 +93,7 @@ void setup() {
   PID_x.SetMode(AUTOMATIC);
   PID_y.SetMode(AUTOMATIC);
   // ============== lidar part =====================
+  /*
   pinMode(LED, OUTPUT);
   digitalWrite(LED, HIGH);
   if (distanceSensor.begin() == 0)
@@ -93,6 +101,12 @@ void setup() {
   distanceSensor.startRanging();
   distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
   digitalWrite(LED, LOW);
+  */ 
+  if (!baro.begin()) {
+    Serial.println("Barometer not found"); 
+    while(1);
+  }
+  baro.setSeaPressure(1016);
 }
 
 
@@ -120,19 +134,19 @@ void loop() {
 
   //if the demo is still ongoing, check to see if there is a desired-color blob
   panel.singleLED(DEBUG_STATE, 10, 0, 0); //standby
-  
+  Serial.println("Standby");
   //========== lidar part =========
   int dist = 0;
   int ball_cap = 0;
-  dist = distanceSensor.getDistance();
-  Serial.println(dist);
-  if ((dist < lidar_thres) && (dist > 0)){ // if we capture the ball 
-    ball_cap = 1; 
-    Serial.println("find ball");
-  }else{
-    ball_cap = 2;
-    Serial.println("no ball");
-  }
+//  dist = distanceSensor.getDistance();
+//  Serial.println(dist);
+//  if ((dist < lidar_thres) && (dist > 0)){ // if we capture the ball 
+//    ball_cap = 1; 
+//    Serial.println("find ball");
+//  }else{
+//    ball_cap = 2;
+//    Serial.println("no ball");
+//  }
   // ========== goal finder =========
   int id = -1;
   int tx = 0; int ty = 0; int tz = 0;
@@ -141,10 +155,19 @@ void loop() {
 
   int x = 0;
   int y = 0; 
+  Serial.println("Calculate Altitude");
+  float altitude = altitudeCalc();
+  Serial.println("Altitude Calculated");
+  if (hoverAltitude > 0){
+	moveVertical(150*(hoverAltitude-altitude));
+  }
+  Serial.println("Moved to hoverAltitude");
   
-  if (ball_cap == 1){ // if we catch the green ball
+//  if (ball_cap == 1){ // if we catch the green ball
     // we go find the goal
+  
   if(cam.exe_goalfinder(goal[0],goal[1],goal[2], id, tx, ty, tz, rx, ry, rz)){
+    Serial.println("Goal Found");
     panel.singleLED(DEBUG_STATE, 0, 10, 0);
     Inputx = tx/1.00;
     Inputy = ty/1.00;
@@ -157,6 +180,11 @@ void loop() {
     Serial.print("y: ");
     Serial.println(Outputy);
     */
+	
+	if (hoverAltitude == -1){
+	  hoverAltitude = altitude;
+	}
+	  Serial.println("hoverAltitude Set");
     Serial.println("moving toward goal");
     moveVertical(Outputy);
     moveHorizontal(Outputx, BASE_SPEED);
@@ -164,7 +192,9 @@ void loop() {
       seeking(); 
       Serial.print("catched the ball and seeking");
     }
+/*
   }else { // we keep searching for the green ball
+  hoverAltitude = -1;
   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   
@@ -184,8 +214,8 @@ void loop() {
     seeking();
     Serial.println("seeking the ball ");
   }
-    }
-
+ }
+*/
 
 
 }
@@ -453,3 +483,8 @@ void debugPIDConstants(int lednum, double oldval, double newval){
     panel.singleLED(lednum, 10, 10, 10);
   }
 }
+
+
+float altitudeCalc(){
+  return (baro.getAltitude() - groundLevel);
+}
diff --git a/Code/Control/Main_Code/.vs/Main_Code/v16/.suo b/Code/Control/Main_Code/.vs/Main_Code/v16/.suo
new file mode 100644
index 0000000000000000000000000000000000000000..8433dddbd650d01c91cbda78ae193bedfecb1970
Binary files /dev/null and b/Code/Control/Main_Code/.vs/Main_Code/v16/.suo differ
diff --git a/Code/Control/Main_Code/.vs/Main_Code/v16/Browse.VC.db b/Code/Control/Main_Code/.vs/Main_Code/v16/Browse.VC.db
new file mode 100644
index 0000000000000000000000000000000000000000..837165e36d8e259d1a8d00631b75fffec7d37b11
Binary files /dev/null and b/Code/Control/Main_Code/.vs/Main_Code/v16/Browse.VC.db differ
diff --git a/Code/Control/Main_Code/.vs/ProjectSettings.json b/Code/Control/Main_Code/.vs/ProjectSettings.json
new file mode 100644
index 0000000000000000000000000000000000000000..0cf5ea5033df16560a10b692341ee93d0ed9f72e
--- /dev/null
+++ b/Code/Control/Main_Code/.vs/ProjectSettings.json
@@ -0,0 +1,3 @@
+{
+  "CurrentProjectSetting": "No Configurations"
+}
\ No newline at end of file
diff --git a/Code/Control/Main_Code/.vs/VSWorkspaceState.json b/Code/Control/Main_Code/.vs/VSWorkspaceState.json
new file mode 100644
index 0000000000000000000000000000000000000000..f814bde9c08cdf9b3c62e3fda1c06e5d546a6c53
--- /dev/null
+++ b/Code/Control/Main_Code/.vs/VSWorkspaceState.json
@@ -0,0 +1,7 @@
+{
+  "ExpandedNodes": [
+    ""
+  ],
+  "SelectedNode": "\\Constants.h",
+  "PreviewInSolutionExplorer": false
+}
\ No newline at end of file
diff --git a/Code/Control/Main_Code/.vs/slnx.sqlite b/Code/Control/Main_Code/.vs/slnx.sqlite
new file mode 100644
index 0000000000000000000000000000000000000000..8e3e7640afad073acdd4ee7734b574de7781eee4
Binary files /dev/null and b/Code/Control/Main_Code/.vs/slnx.sqlite differ
diff --git a/Code/Control/Main_Code/ColorDetect.cpp b/Code/Control/Main_Code/ColorDetect.h
similarity index 89%
rename from Code/Control/Main_Code/ColorDetect.cpp
rename to Code/Control/Main_Code/ColorDetect.h
index 8805715e40be4b1478b45f039c9ea64d22be1f55..282f542f9bc6ee1ade32513236ae80658da33548 100644
--- a/Code/Control/Main_Code/ColorDetect.cpp
+++ b/Code/Control/Main_Code/ColorDetect.h
@@ -1,6 +1,14 @@
+#ifndef COLORDETECT_H
+#define COLORDETECT_H
+
 #include <string>
+using namespace std;
+
+// Function Declarations
+void translateCodetoThreshold(int code);
+void setColorThreshold(String msg, int8_t thres[], int arraySize);
 
-void ColorDetect_Setup(){
+void ColorDetect_Setup(int findcolor){
   translateCodetoThreshold(findcolor);
 }
 
@@ -75,3 +83,5 @@ void setColorThreshold(String msg, int8_t thres[], int arraySize){
     }
   }
 }
+
+#endif
diff --git a/Code/Control/Main_Code/Constants.cpp b/Code/Control/Main_Code/Constants.cpp
deleted file mode 100644
index f332f0389bf86d358802ccaa55e7df9392cdd206..0000000000000000000000000000000000000000
--- a/Code/Control/Main_Code/Constants.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-// Identify all the global constants that will be used by the robot
-const int BAUD_RATE = 115200;
-const int MAX_SPEED = 255;
-const int SEEKING_SPEED = 70;
-const double RESOLUTION_W = 320.0;
-const double RESOLUTION_H = 240.0;
-const int ENDDEMO_TAG = 0;
-const uint32_t THISNODE_ID = 88789821;
-int BASE_SPEED = 50; //125;
\ No newline at end of file
diff --git a/Code/Control/Main_Code/Constants.h b/Code/Control/Main_Code/Constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..f43c1dcc4711ba616c0028a540b018221e5fa0a6
--- /dev/null
+++ b/Code/Control/Main_Code/Constants.h
@@ -0,0 +1,30 @@
+#ifndef CONSTANTS_H
+#define CONSTANTS_H
+
+#include <vector>
+
+// Identify all the global constants that will be used by the robot
+const int BAUD_RATE = 115200;
+const int MAX_SPEED = 255;
+const int SEEKING_SPEED = 70;
+const double RESOLUTION_W = 320.0;
+const double RESOLUTION_H = 240.0;
+const int ENDDEMO_TAG = 0;
+const uint32_t THISNODE_ID = 88789821;
+int BASE_SPEED = 50; //125;
+
+// The debug LEDs that we will be using. Description at:
+const int DEBUG_STATE = 31;
+const int DEBUG_KP = 30;
+const int DEBUG_KI = 29;
+const int DEBUG_KD = 28;
+const int DEBUG_BASESPEED = 27;
+const int DEBUG_THRESHOLD_MIN = 26;
+const int DEBUG_THRESHOLD_MAX = 25;
+const int DEBUG_VERTICALSPEED = 17;
+const int DEBUG_RSPEED = 16;
+const int DEBUG_LSPEED = 24;
+
+int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
+
+#endif
diff --git a/Code/Control/Main_Code/Debug.cpp b/Code/Control/Main_Code/Debug.h
similarity index 72%
rename from Code/Control/Main_Code/Debug.cpp
rename to Code/Control/Main_Code/Debug.h
index c29f46989f33b37a320335f2ebf0c61dd75b9b16..4b513129c5d39174d4efbaa836640c10528f3a1a 100644
--- a/Code/Control/Main_Code/Debug.cpp
+++ b/Code/Control/Main_Code/Debug.h
@@ -1,19 +1,13 @@
+#ifndef DEBUG_H
+#define DEBUG_H
+
 #include "LedPanel.h"
-#include "Constants.cpp"
-
-LedPanel panel(NUMPIXELS,PIN_PIXELS);
-
-//The debug LEDs that we will be using. Description at:
-const int DEBUG_STATE = 31;
-const int DEBUG_KP = 30;
-const int DEBUG_KI = 29;
-const int DEBUG_KD = 28;
-const int DEBUG_BASESPEED = 27;
-const int DEBUG_THRESHOLD_MIN = 26;
-const int DEBUG_THRESHOLD_MAX = 25;
-const int DEBUG_VERTICALSPEED = 17;
-const int DEBUG_RSPEED = 16;
-const int DEBUG_LSPEED = 24;
+#include "Constants.h"
+
+LedPanel panel(NUMPIXELS, PIN_PIXELS);
+
+// Function Declarations
+void debugPIDConstants(int lednum, double oldval, double newval);
 
 void Debug_Setup(){
   panel.beginPanel();
@@ -44,6 +38,23 @@ void Debug_GoalSeeking(){
   panel.singleLED(DEBUG_STATE, 10, 10, 10);
 }
 
+void Debug_SetPIDConstants(double &p_constant, double &i_constant, double &d_constant, double new_p, double new_i, double new_d){
+  unsigned long t = millis();
+  debugPIDConstants(DEBUG_KP, p_constant, new_p);
+  debugPIDConstants(DEBUG_KI, i_constant, new_i);
+  debugPIDConstants(DEBUG_KD, d_constant, new_d);
+
+  p_constant = new_p;
+  i_constant = new_i;
+  d_constant = new_d;
+  
+  while(millis() < t+2000); //debugging LED will light up for 2 seconds
+  
+  panel.singleLED(DEBUG_KP, 0, 0, 0);
+  panel.singleLED(DEBUG_KI, 0, 0, 0);
+  panel.singleLED(DEBUG_KD, 0, 0, 0);
+}
+
 void displaySpeed(int lspeed, int rspeed){
   //Serial.println("display speed");
   if (lspeed < 0){
@@ -84,3 +95,5 @@ void debugPIDConstants(int lednum, double oldval, double newval){
     panel.singleLED(lednum, 10, 10, 10);
   }
 }
+
+#endif
diff --git a/Code/Control/Main_Code/LIDAR.cpp b/Code/Control/Main_Code/LIDAR.h
similarity index 95%
rename from Code/Control/Main_Code/LIDAR.cpp
rename to Code/Control/Main_Code/LIDAR.h
index 4c08a21ef9b5e9a23aff4f3a4899fe9d5bb15607..c5cab0b977c934e1ab015e9af3204a8e3efe9a8d 100644
--- a/Code/Control/Main_Code/LIDAR.cpp
+++ b/Code/Control/Main_Code/LIDAR.h
@@ -1,3 +1,6 @@
+#ifndef LIDAR_H
+#define LIDAR_H
+
 #include <SparkFun_VL53L1X.h>
 
 // LIDAR
@@ -30,3 +33,5 @@ float LIDAR_BallCapture(){
     Serial.println("no ball");
   }
 }
+
+#endif
diff --git a/Code/Control/Main_Code/Localization.cpp b/Code/Control/Main_Code/Localization.h
similarity index 54%
rename from Code/Control/Main_Code/Localization.cpp
rename to Code/Control/Main_Code/Localization.h
index ee447c42e4c2e811ece202818827a32a4d1319c3..54be7c93add1bc0bdc5c44af35c52297d78d7c78 100644
--- a/Code/Control/Main_Code/Localization.cpp
+++ b/Code/Control/Main_Code/Localization.h
@@ -1,4 +1,7 @@
-#include "Constants.cpp"
+#ifndef LOCALIZATION_H
+#define LOCALIZATION_H
+
+#include "Constants.h"
 #include <Adafruit_MPL3115A2.h>
 
 float groundLevel = 120;
@@ -8,7 +11,12 @@ float ceilingHeight = 20;
 
 Adafruit_MPL3115A2 baro;
 
-void LocalizationSetup(){
+float hoverAltitude = -1;
+
+// Function Declarations
+float altitudeCalc();
+
+void Localization_Setup(){
   if (!baro.begin()) {
     Serial.println("Barometer not found");
     while(1);
@@ -16,16 +24,27 @@ void LocalizationSetup(){
   baro.setSeaPressure(1016);
 }
 
-void LocalizationInit(){
+float Localization_Init(){
   float altitude = altitudeCalc() - groundLevel;
+  return altitude;
 }
 
-void Localization_GoalFound(){
+void Localization_GoalFound(float altitude){
   if (hoverAltitude == -1){
     hoverAltitude = altitude;
   }
 }
 
+void Localization_BallDetected(){
+  hoverAltitude = -1;
+}
+
+float altitudeControl(){
+  return hoverAltitude;
+}
+
 float altitudeCalc(){
   return baro.getAltitude();
 }
+
+#endif
diff --git a/Code/Control/Main_Code/Main_Code.ino b/Code/Control/Main_Code/Main_Code.ino
index bb99f2fa6c4575515b43f77f009244c912f998bb..a359a37f7e217b7aca69bf361bd0f3e3d9e372b5 100644
--- a/Code/Control/Main_Code/Main_Code.ino
+++ b/Code/Control/Main_Code/Main_Code.ino
@@ -1,23 +1,34 @@
+#include "PID.h"
 #include <string>
 #include <vector>
 #include <Wire.h>
+#include "Debug.h"
+#include "LIDAR.h"
 #include "Camera.h"
-#include "Constants.cpp"
 #include <Arduino.h>
+#include "Constants.h"
 #include "utilities.h"
+#include "Propulsion.h"
+#include "ColorDetect.h"
+#include "Localization.h"
 // #include "Adafruit_VL53L0X.h"
+using namespace std;
 
 //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
 char newPTCL = '1';
-int pause = 0;
+int pauseState = 0;
 int displayTracking = 0;
-int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
 
 int ballCapture = 0;
 int ballDetect  = 0;
 int goalFind    = 0;
 
+float altitude  = 0;
+
+// Function Declarations
+
+
 //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
@@ -30,26 +41,26 @@ void setup() {
   AFMS.begin();  // create with the default frequency 1.6KHz
   interface.begin(); //communication between ESP and OpenMV
     
-  ColorDetect_Setup();
+  ColorDetect_Setup(findcolor);
   Debug_Setup();
   PID_Setup();
   LIDAR_Setup();
   Localization_Setup();
-  #ifdef MESH_CPP
+  #ifdef MESH_H
     Mesh_Setup();
   #endif
 }
 
-void loop() {  
+void loop(){
   Debug_LoopStart();
-  #ifdef MESH_CPP
+  #ifdef MESH_H
     MeshUpdate();
   #endif
   
   /* --------- PAUSE STATE --------- */
-  while(pause == 1){
+  while(pauseState == 1){
     Serial.println("pause");
-    #ifdef MESH_CPP
+    #ifdef MESH_H
       Mesh_PauseState();
     #endif
     Debug_PauseState();
@@ -66,11 +77,13 @@ void loop() {
   /* -------- STANDBY STATE -------- */
   Debug_StandbyState();
   /* ------ END STANDBY STATE ------ */
+
+  altitude = Localization_Init();
   
   /* --------- INTERRUPTS ---------- */
-  altitudeBarrier();
+  altitudeBarrier(altitude);
   
-  if(insideWindyArea){
+  if(insideWindyArea()){
     // leave windy area
     // flip horizontal inputs
   }
@@ -86,7 +99,7 @@ void loop() {
 	  if(goalFind){ // Ball captured and Goal found
       Debug_GoalFound();
       PID_GoalFound(tx/1.00, ty/1.00);
-      Localization_GoalFound();
+      Localization_GoalFound(altitude);
 	    Propulsion_GoalFound();
     }
     else { // Ball captured but Goal not found
@@ -97,6 +110,7 @@ void loop() {
   else { // Ball not captured
 	  if(ballDetect) { // Ball not captured but detected
       Debug_BallDetected();
+      Localization_BallDetected();
       PID_BallDetected(x/1.00, y/1.00);
       Propulsion_BallDetected();
     }
@@ -116,3 +130,7 @@ void altitudeBarrier(int altitude){
     moveVertical(((ceilingHeight - highHeightBarrier) - altitude)*255/highHeightBarrier);
   }	
 }
+
+float insideWindyArea(){
+  return 0;
+}
diff --git a/Code/Control/Main_Code/Mesh.cpp.bkp b/Code/Control/Main_Code/Mesh.h.bkp
similarity index 100%
rename from Code/Control/Main_Code/Mesh.cpp.bkp
rename to Code/Control/Main_Code/Mesh.h.bkp
diff --git a/Code/Control/Main_Code/PID.cpp b/Code/Control/Main_Code/PID.h
similarity index 85%
rename from Code/Control/Main_Code/PID.cpp
rename to Code/Control/Main_Code/PID.h
index 1cd5a69da6159b85ccdcc04560c68adddeb2ff6e..fa548cec703a7b011750665077df4d4ec9487b3c 100644
--- a/Code/Control/Main_Code/PID.cpp
+++ b/Code/Control/Main_Code/PID.h
@@ -1,3 +1,9 @@
+#ifndef PID_H
+#define PID_H
+
+#include <string>
+#include "Debug.h"
+#include "Constants.h"
 #include <PID_v1.h>
 
 double Setpointx, Inputx, Outputx;
@@ -9,6 +15,9 @@ double Kpy=2, Kiy=0.1,  Kdy=0.25;
 PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
 PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
 
+// Function Declarations
+void Debug_SetPIDConstants(double &p_constant, double &i_constant, double &d_constant, double new_p, double new_i, double new_d);
+
 void PID_Setup(){
   Setpointx = 160.0; 
   Setpointy = 120.0; //the values that the PID will try to reach
@@ -25,10 +34,10 @@ void PID_GoalFound(int InputxVal, int InputyVal){
   PID_x.Compute();
   PID_y.Compute();
 
-  Serial.print("Outputx: ");
-  Serial.println(Outputx);
-  Serial.print("Outputy: ");
-  Serial.println(Outputy);
+//  Serial.print("Outputx: ");
+//  Serial.println(Outputx);
+//  Serial.print("Outputy: ");
+//  Serial.println(Outputy);
 }
 
 void PID_BallDetected(int InputxVal, int InputyVal){
@@ -111,18 +120,8 @@ void setPIDConstants(String msg, double &p_constant, double &i_constant, double
     }
   }
 
-  //DEBUGGING SECTION
-  unsigned long t = millis();
-  debugPIDConstants(DEBUG_KP, p_constant, new_p);
-  debugPIDConstants(DEBUG_KI, i_constant, new_i);
-  debugPIDConstants(DEBUG_KD, d_constant, new_d);
-
-  p_constant = new_p;
-  i_constant = new_i;
-  d_constant = new_d;
-  
-  while(millis() < t+2000); //debugging LED will light up for 2 seconds
-  panel.singleLED(DEBUG_KP, 0, 0, 0);
-  panel.singleLED(DEBUG_KI, 0, 0, 0);
-  panel.singleLED(DEBUG_KD, 0, 0, 0);
+  // DEBUGGING SECTION
+  Debug_SetPIDConstants(p_constant, i_constant, d_constant, new_p, new_i, new_d);
 }
+
+#endif
diff --git a/Code/Control/Main_Code/Propulsion.cpp b/Code/Control/Main_Code/Propulsion.h
similarity index 91%
rename from Code/Control/Main_Code/Propulsion.cpp
rename to Code/Control/Main_Code/Propulsion.h
index 0854722400def327caea86765bbf8af23e5cfb55..663c317437948ca2ae82103495dc9c5a66b8891d 100644
--- a/Code/Control/Main_Code/Propulsion.cpp
+++ b/Code/Control/Main_Code/Propulsion.h
@@ -1,8 +1,15 @@
-#include "Constants.cpp"
+#ifndef PROPULSION_H
+#define PROPULSION_H
+
+#include "Constants.h"
 #include <Adafruit_MotorShield.h>
 
 int zero = 0;
 
+// Function Declarations
+void moveVertical(int vel);
+void moveHorizontal(int vel_hori, int base_speed);
+
 // Create the motor shield object with the default I2C address
 Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
 Adafruit_DCMotor *motor_VL = AFMS.getMotor(2); 
@@ -75,3 +82,5 @@ void moveHorizontal(int vel_hori, int base_speed){
   motor_HL->setSpeed(min(MAX_SPEED, abs(rspeed)));
   motor_HR->setSpeed(min(MAX_SPEED, abs(lspeed)));
 }
+
+#endif
diff --git a/Code/Control/Main_Code/utilities.h b/Code/Control/Main_Code/utilities.h
index a1ad9adbad7124479fd8d5168a7b1b24a915521c..cfd5b6b6ed97c4e3ea630fe2f7494f8fdb298e61 100644
--- a/Code/Control/Main_Code/utilities.h
+++ b/Code/Control/Main_Code/utilities.h
@@ -1,8 +1,7 @@
 #ifndef UTI_H
 #define UTI_H
 
-void lab2rgb( float l_s, float a_s, float b_s, float& R, float& G, float& B );
+void lab2rgb(float l_s, float a_s, float b_s, float& R, float& G, float& B );
 void translateCodetoThreshold(int code);
 
-
 #endif