diff --git a/.vscode/settings.json b/.vscode/settings.json
index fc4c15b..df1ab68 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,3 +1,60 @@
 {
-    "C_Cpp.errorSquiggles": "enabled"
+  "C_Cpp.errorSquiggles": "enabled",
+  "C_Cpp_Runner.cCompilerPath": "gcc",
+  "C_Cpp_Runner.cppCompilerPath": "g++",
+  "C_Cpp_Runner.debuggerPath": "gdb",
+  "C_Cpp_Runner.cStandard": "",
+  "C_Cpp_Runner.cppStandard": "",
+  "C_Cpp_Runner.msvcBatchPath": "C:/Program Files/Microsoft Visual Studio/VR_NR/Community/VC/Auxiliary/Build/vcvarsall.bat",
+  "C_Cpp_Runner.useMsvc": false,
+  "C_Cpp_Runner.warnings": [
+    "-Wall",
+    "-Wextra",
+    "-Wpedantic",
+    "-Wshadow",
+    "-Wformat=2",
+    "-Wcast-align",
+    "-Wconversion",
+    "-Wsign-conversion",
+    "-Wnull-dereference"
+  ],
+  "C_Cpp_Runner.msvcWarnings": [
+    "/W4",
+    "/permissive-",
+    "/w14242",
+    "/w14287",
+    "/w14296",
+    "/w14311",
+    "/w14826",
+    "/w44062",
+    "/w44242",
+    "/w14905",
+    "/w14906",
+    "/w14263",
+    "/w44265",
+    "/w14928"
+  ],
+  "C_Cpp_Runner.enableWarnings": true,
+  "C_Cpp_Runner.warningsAsError": false,
+  "C_Cpp_Runner.compilerArgs": [],
+  "C_Cpp_Runner.linkerArgs": [],
+  "C_Cpp_Runner.includePaths": [],
+  "C_Cpp_Runner.includeSearch": [
+    "*",
+    "**/*"
+  ],
+  "C_Cpp_Runner.excludeSearch": [
+    "**/build",
+    "**/build/**",
+    "**/.*",
+    "**/.*/**",
+    "**/.vscode",
+    "**/.vscode/**"
+  ],
+  "C_Cpp_Runner.useAddressSanitizer": false,
+  "C_Cpp_Runner.useUndefinedSanitizer": false,
+  "C_Cpp_Runner.useLeakSanitizer": false,
+  "C_Cpp_Runner.showCompilationTime": false,
+  "C_Cpp_Runner.useLinkTimeOptimization": false,
+  "C_Cpp_Runner.msvcSecureNoWarnings": false
 }
\ No newline at end of file
diff --git a/include/config.h b/include/config.h
index 2a14be6..595939d 100644
--- a/include/config.h
+++ b/include/config.h
@@ -47,4 +47,4 @@ const uint16_t MAX_SPEED = 1550;
 
 // Other constants
 const int16_t TAKEOFF_HEIGHT_MM = 1000; // Target height for takeoff in mm
-const int16_t LANDING_DISTANCE_MM = 200; // Distance from sensor that triggers landing sequence
\ No newline at end of file
+const int16_t LANDING_DISTANCE_MM = 40; // Distance from sensor that triggers landing sequence
\ No newline at end of file
diff --git a/lib/motor_control/motor_control.cpp b/lib/motor_control/motor_control.cpp
index 5fb10dd..18fb9e8 100644
--- a/lib/motor_control/motor_control.cpp
+++ b/lib/motor_control/motor_control.cpp
@@ -119,18 +119,36 @@ void balanceRoll(float accel_y_g, float gyro_y_dps){
 void balanceAltitude(float pressure, float hoverPressure){
   //TODO: Derivative control of altitude balancing
 
-  /* Method 1: if it's falling, move it */
+  int16_t pressureError = hoverPressure - pressure;
   if(pressure < (hoverPressure - PRESSURE_THRESHOLD)){ // drone is falling
-    changeSpeed(3);
+    changeSpeed(map(pressureError, 
+                    0, hoverPressure + (ALTITUDE_THRESHOLD_MM*5)*PRESSURE_CHANGE_TO_ALTITUDE_MM, 
+                    1, MAX_CHANGE/2)); // Proportional control based on distance from target height
   }
   else if(pressure > (hoverPressure + PRESSURE_THRESHOLD)){ // drone is rising
-    changeSpeed(-3);
+    changeSpeed(map(pressureError, 
+                    -hoverPressure + (ALTITUDE_THRESHOLD_MM*5)*PRESSURE_CHANGE_TO_ALTITUDE_MM, 0, 
+                    -MAX_CHANGE/2, -1)); // Proportional control based on distance from target height
   }
 
   /* Method 2: Check exact altitude relative to ground */
 
 }
 
+void takeOff(int16_t distance_mm){
+  // Gradually increase speed based on distance from target height
+  static int16_t lastError = TAKEOFF_HEIGHT_MM;
+  uint16_t error = TAKEOFF_HEIGHT_MM - distance_mm;
+
+  // Proportional control based on distance from target height
+  int16_t change = TAKEOFF_SPEED * ((error)/TAKEOFF_HEIGHT_MM) + 2;
+  // Derivative control based on change in distance from target height
+  change += map(error - lastError, -TAKEOFF_HEIGHT_MM, TAKEOFF_HEIGHT_MM, -MAX_CHANGE, MAX_CHANGE);
+
+  lastError = error;
+  changeSpeed(change);
+}
+
 // void land(){
 // }
 
diff --git a/lib/motor_control/motor_control.h b/lib/motor_control/motor_control.h
index 23d3b0f..791655c 100644
--- a/lib/motor_control/motor_control.h
+++ b/lib/motor_control/motor_control.h
@@ -16,7 +16,7 @@ void balancePitch(float accel_x_g, float gyro_x_dps); // Balances motors based o
 void balanceRoll(float accel_y_g, float gyro_y_dps); // Balances motors based on roll
 void balanceAltitude(float pressure, float hoverPressure); // Checks for changes in verticle position, adjusts all motors based on it
 
-void takeOff(); // Takeoff sequence
+void takeOff(int16_t distance_mm); // Takeoff sequence
 
 //TODO: Move land and forceLand over here from main
 // void land(); // Landing sequence
diff --git a/src/main.cpp b/src/main.cpp
index 3fadddc..bbdc331 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -69,7 +69,8 @@ void stateMachine(){
     count = 0;
   }
 
-  switch (currentState){ // State changes
+  /* ---------- State Changes ---------- */
+  switch (currentState){
     case OFF: 
       if(digitalRead(DRONE_POWER)){
         currentState = INIT;
@@ -88,7 +89,8 @@ void stateMachine(){
       break;
 
     case HOVERING:
-      if(sData.distance_mm < LANDING_DISTANCE_MM){
+      if(sData.distance_mm < LANDING_DISTANCE_MM || sData.pressure.pressure > TAKEOFF_HEIGHT_MM*2*PRESSURE_CHANGE_TO_ALTITUDE_MM + basePressure){ 
+        // If drone is close to ground, too high, or pressure sensor is malfunctioning, land
         currentState = LANDING;
       } 
       // else if(!isStable(msg)){
@@ -109,7 +111,8 @@ void stateMachine(){
       break;
   }
 
-  switch (currentState){ // State Actions
+  /* ---------- State Actions ---------- */
+  switch (currentState){ 
     case OFF:
       digitalWrite(LED_PIN, HIGH);
       stopMotors();
@@ -128,7 +131,10 @@ void stateMachine(){
     
     case TAKEOFF:
       readPressure(sData);
-      takeOff();
+      takeOff(sData.distance_mm);
+      balancePitch(sData.accel_x_g, sData.gyro_x_dps);
+      balanceRoll(sData.accel_y_g, sData.gyro_y_dps);
+      writeESCs();
       break;
     
     case HOVERING:
@@ -235,10 +241,6 @@ void sendReadings(){ // Send readings to Saleae
 }
 
 /* ----- MOTOR CONTROL FUNCTIONS ----- */
-void takeOff(){
-  changeSpeed(TAKEOFF_SPEED * ((TAKEOFF_HEIGHT_MM - sData.distance_mm)/TAKEOFF_HEIGHT_MM) + 1); // Gradually increase speed
-  // hehehehaw
-}
 
 void land(){ // Landing sequence
   readPressure(sData);
@@ -279,6 +281,7 @@ void rcISR(){
   moveY(msg.y_change);
   hoverPressure += msg.z_change;
 }
+
 /* ----- Testing Functions ----- */
 
 /* ----- Additional Functions -----*/