diff --git a/include/driver/motorButton.hpp b/include/driver/motorButton.hpp
index 47204b41..4614151c 100644
--- a/include/driver/motorButton.hpp
+++ b/include/driver/motorButton.hpp
@@ -10,7 +10,16 @@ namespace Pronounce {
     private:
         bool goToImmediately = false;
 
-        bool autonomousButton = false;
+        bool autonomousPosition = false;
+
+		bool dejam = false;
+		double dejamTime = 0;
+		double dejamSpeed = 0;
+		double dejamAuthority = 0;
+		double dejamStartTime = 0;
+		double jamSpeed = 50;
+
+		bool jammed = false;
 
         int positiveAuthority = 0;
         int neutralAuthority = 0;
@@ -18,11 +27,18 @@ namespace Pronounce {
 
         int autonomousAuthority = 0;
 
+		bool goToHeight = false;
+		double height;
+		double length;
+
+		double gearRatio;
+
         int min = 0;
         int max = 0;
         pros::Motor* motor;
 
     public:
+        MotorButton();
         MotorButton(pros::Controller* controller, pros::Motor* motor,
         pros::controller_digital_e_t positiveButton = pros::E_CONTROLLER_DIGITAL_L1,
         pros::controller_digital_e_t negativeButton = pros::E_CONTROLLER_DIGITAL_L2,
@@ -31,10 +47,13 @@ namespace Pronounce {
         int negativeAuthority = 0,
         int min = 0,
         int max = 0);
-        MotorButton();
         
         void updateActuator();
 
+		void resetPosition(double currentPosition) {
+			this->motor->set_zero_position(-currentPosition);
+		}
+
         bool getGoToImmediately() {
             return this->goToImmediately;
         }
@@ -51,16 +70,91 @@ namespace Pronounce {
             this->autonomousAuthority = autonomousAuthority;
         }
 
-        bool getAutonomousButton() {
-            return this->autonomousButton;
+        bool getAutonomousPosition() {
+            return this->autonomousPosition;
         }
 
-        void setAutonomousButton(bool autonomousButton) {
-            this->autonomousButton = autonomousButton;
+        void setAutonomousPosition(bool autonomousPosition) {
+            this->autonomousPosition = autonomousPosition;
         }
 
+		bool getGoToHeight() {
+			return this->goToHeight;
+		}
+
+		void setGoToHeight(bool goToHeight) {
+			this->goToHeight = goToHeight;
+		}
+
+		double getLength(double length) {
+			return this->length;
+		}
+
+		void setLength(double length) {
+			this->length = length;
+		}
+
+		double getHeight() {
+			return height;
+		}
+
+		void setHeight(double height) {
+			this->height = height;
+		}
+
+		double getGearRatio() {
+			return gearRatio;
+		}
+
+		void setGearRatio(double gearRatio) {
+			this->gearRatio = gearRatio;
+		}
+		
+		bool getJammed() {
+			return this->jammed;
+		}
+
+		void setJammed(bool jammed) {
+			this->jammed = jammed;
+		}
+
+		bool getDejam() {
+			return this->dejam;
+		}
+
+		void setDejam(bool dejam) {
+			this->dejam = dejam;
+		}
+
+		double getDejamTime() {
+			return this->dejamTime;
+		}
+
+		void setDejamTime(double dejamTime) {
+			this->dejamTime = dejamTime;
+		}
+
+		double getDejamAuthority() {
+			return this->dejamAuthority;
+		}
+
+		void setDejamAuthority(double dejamAuthority) {
+			this->dejamAuthority = dejamAuthority;
+		}
+
+		double getDejamSpeed() {
+			return this->dejamSpeed;
+		}
+
+		void setDejamSpeed(double dejamSpeed) {
+			this->dejamSpeed = dejamSpeed;
+		}
+
+		double getDejamStartTime() {
+			return this->dejamStartTime;
+		}
+
         ~MotorButton();
     };
 
 } // namespace Pronounce
-
diff --git a/include/feedbackControllers/bangBang.hpp b/include/feedbackControllers/bangBang.hpp
new file mode 100644
index 00000000..29915f42
--- /dev/null
+++ b/include/feedbackControllers/bangBang.hpp
@@ -0,0 +1,65 @@
+#pragma once
+
+#include "utils/utils.hpp"
+
+namespace Pronounce {
+	class BangBang {
+	private:
+		double minimumDifference;
+		bool reversable;
+		double outputStrength;
+		double setPoint = 0;
+		double lastInput = 0;
+	public:
+		BangBang();
+		BangBang(double minimumDifference);
+		BangBang(double minimumDifference, bool reversable);
+		BangBang(double minimumDifference, bool reversable, double outputStrength);
+
+		/**
+		 * @brief Updates the controller, input the current value, and it will return the output
+		 * 
+		 * @param input The current state
+		 * @return double The output at that state
+		 */
+		double update(double input);
+
+		double getSetPoint() {
+			return setPoint;
+		}
+
+		void setSetPoint(double setPoint) {
+			this->setPoint = setPoint;
+		}
+
+		double getMinimumDifference() {
+			return minimumDifference;
+		}
+
+		void setMinimumDifference(double minimumDifference) {
+			this->minimumDifference = minimumDifference;
+		}
+
+		bool isReversable() {
+			return reversable;
+		}
+
+		void setReversable(bool reversable) {
+			this->reversable = reversable;
+		}
+
+		double getOutputStrength() {
+			return outputStrength;
+		}
+
+		void setOutputStrength(double outputStrength) {
+			this->outputStrength = outputStrength;
+		}
+
+		bool isInRange() {
+			return abs(this->setPoint - this->lastInput) > minimumDifference;
+		}
+
+		~BangBang();
+	};
+} // namespace Pronounce
\ No newline at end of file
diff --git a/include/pid/pid.hpp b/include/feedbackControllers/pid.hpp
similarity index 100%
rename from include/pid/pid.hpp
rename to include/feedbackControllers/pid.hpp
diff --git a/include/main.h b/include/main.h
index b4a52abd..df18a396 100644
--- a/include/main.h
+++ b/include/main.h
@@ -68,7 +68,12 @@
 #include "driver/motorButton.hpp"
 #include "driver/solenoidButton.hpp"
 
+// FeedbackControllers
+#include "feedbackControllers/bangBang.hpp"
+#include "feedbackControllers/pid.hpp"
+
 // Motion control
+#include "motionControl/balance.hpp"
 #include "motionControl/purePursuit.hpp"
 #include "motionControl/omniPurePursuit.hpp"
 #include "motionControl/tankPurePursuit.hpp"
@@ -79,9 +84,6 @@
 #include "odometry/threeWheelOdom.hpp"
 #include "odometry/tankOdom.hpp"
 
-// PID
-#include "pid/pid.hpp"
-
 // Position
 #include "position/motorOdom.hpp"
 #include "position/odomWheel.hpp"
diff --git a/include/motionControl/balance.hpp b/include/motionControl/balance.hpp
new file mode 100644
index 00000000..4a0a8b16
--- /dev/null
+++ b/include/motionControl/balance.hpp
@@ -0,0 +1,81 @@
+#pragma once
+
+#include "feedbackControllers/bangBang.hpp"	
+#include "feedbackControllers/pid.hpp"
+#include "chassis/abstractTankDrivetrain.hpp"
+#include "api.h"
+
+namespace Pronounce {
+	class Balance
+	{
+	private:
+		BangBang* linearController;
+		PID* orientationController;
+
+		bool enabled = false;
+
+		AbstractTankDrivetrain* drivetrain;
+		pros::Imu* imu;
+	public:
+		Balance();
+		Balance(AbstractTankDrivetrain* drivetrain);
+		Balance(AbstractTankDrivetrain* drivetrain, pros::Imu* imu);
+		Balance(AbstractTankDrivetrain* drivetrain, pros::Imu* imu, BangBang* linearController, PID* orientationController);
+
+		void update();
+
+		void balance(double waitTime) {
+			while(!this->isBalanced()) {
+				this->update();
+				pros::Task::delay(waitTime);
+			}
+		}
+
+		bool isBalanced() {
+			return linearController->isInRange();
+		}
+
+		BangBang* getLinearController() {
+			return this->linearController;
+		}
+
+		void setLinearController(BangBang* linearController) {
+			this->linearController = linearController;
+		}
+
+		PID* getOrientationController() {
+			return this->orientationController;
+		}
+
+		void setOrientationController(PID* orientationController) {
+			this->orientationController = orientationController;
+		}
+
+		bool isEnabled() {
+			return this->enabled;
+		}
+
+		void setEnabled(bool enabled) {
+			this->enabled = enabled;
+		}
+
+		AbstractTankDrivetrain* getDrivetrain() {
+			return this->drivetrain;
+		}
+
+		void setDrivetrain(AbstractTankDrivetrain* drivetrain) {
+			this->drivetrain = drivetrain;
+		}
+
+		pros::Imu* getImu() {
+			return this->imu;
+		}
+
+		void setImu(pros::Imu* imu) {
+			this->imu = imu;
+		}
+
+
+		~Balance();
+	};
+} // namespace Pronounce
diff --git a/include/motionControl/purePursuit.hpp b/include/motionControl/purePursuit.hpp
index 87bba47f..b9d113e4 100644
--- a/include/motionControl/purePursuit.hpp
+++ b/include/motionControl/purePursuit.hpp
@@ -4,7 +4,7 @@
 #include <math.h>
 #include <algorithm>
 #include <iostream>
-#include "pid/pid.hpp"
+#include "feedbackControllers/pid.hpp"
 #include "utils/path.hpp"
 #include "utils/position.hpp"
 #include "utils/utils.hpp"
diff --git a/include/utils/purePursuitProfile.hpp b/include/utils/purePursuitProfile.hpp
index a7832bf8..b1fdcb6b 100644
--- a/include/utils/purePursuitProfile.hpp
+++ b/include/utils/purePursuitProfile.hpp
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "pid/pid.hpp"
+#include "feedbackControllers/pid.hpp"
 
 namespace Pronounce {
     class PurePursuitProfile {
diff --git a/pathTest.cpp b/pathTest.cpp
index fdb1b3d1..e9a0c918 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -26,8 +26,8 @@
 #include "src/utils/purePursuitProfile.cpp"
 #include "include/utils/purePursuitProfileManager.hpp"
 #include "src/utils/purePursuitProfileManager.cpp"
-#include "include/pid/pid.hpp"
-#include "src/pid/pid.cpp"
+#include "include/feedbackControllers/pid.hpp"
+#include "src/feedbackControllers/pid.cpp"
 #include "include/chassis/abstractDrivetrain.hpp"
 #include "src/chassis/abstractDrivetrain.cpp"
 #include "include/chassis/abstractTankDrivetrain.hpp"
@@ -60,13 +60,13 @@ double playbackMultiplier = 1;
 #define yOffset 30
 #define multiplier 3
 
-#define lookahead 10
+#define lookahead 8
 
 #define starting_point_random 1
 
 // ANCHOR Printing variables
 
-#define PRINT_LIVE true
+#define PRINT_LIVE false
 #define GRAPH true
 
 #define FIELD_WIDTH 140.6
@@ -292,7 +292,7 @@ int main() {
 	SimTankDrivetrain drivetrain(15.0, 15.0/fps, 60.0/fps);
 	SimOdometry odometry(&drivetrain);
 
-	TankPurePursuit purePursuit(&drivetrain, &odometry, 10);
+	TankPurePursuit purePursuit(&drivetrain, &odometry, lookahead);
 
 	double trackWidth = drivetrain.getTrackWidth(); 
 
@@ -310,38 +310,109 @@ int main() {
 	testPathIndex = purePursuit.addPath(testPath.getPath(0.1));
 	*/
 
+	// Left home zone to left neutral goal
+	QuadraticSplinePath leftHomeZoneToLeftNeutralGoal;
 
-	QuadraticSplinePath leftAllianceToRightHomeZone = QuadraticSplinePath();
+	leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(23.2, 11.4), Vector(30, M_PI_4*1.5)));
+	leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(48, 46.8), Vector(20, 0)));
+	leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(35, 66), Vector(20, 0)));
 
-	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(30.0, 11.4), Vector(15.0, M_PI_2)));
-	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(15.2, 35.0), Vector(15.0, 0.0)));
-	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(46.4, 46.4), Vector(15.0, M_PI_2)));
-	leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(93.9, 46.4), Vector(15.0, M_PI_2)));
+	int path = purePursuit.addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.01));
 
-	leftAllianceToRightHomeZoneIndex = purePursuit.addPath(leftAllianceToRightHomeZone.getPath(0.1));
+	QuadraticSplinePath leftNeutralGoalToFarHomeZone;
 
-	QuadraticSplinePath rightHomeZoneToRightAlliance = QuadraticSplinePath();
+	leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 66), Vector(30, -M_PI_4)));
+	leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(70.3, 93.9), Vector(30, 0)));
+	leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(70.3, 107), Vector(20, 0)));
 
-	rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(93.9, 46.4), Vector(15.0, -M_PI_2)));
-	rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123.0, 40.0), Vector(15.0, - M_PI_2 - M_PI_4)));
+	int path2 = purePursuit.addPath(leftNeutralGoalToFarHomeZone.getPath(0.01));
 
-	rightHomeZoneToRightAllianceIndex = purePursuit.addPath(rightHomeZoneToRightAlliance.getPath(0.1));
+	QuadraticSplinePath farHomeZoneToMidNeutralGoal;
 
-	QuadraticSplinePath rightAllianceToRightRings = QuadraticSplinePath();
+	farHomeZoneToMidNeutralGoal.addPoint(SplinePoint(Point(70.3, 107), Vector(20, 0)));
+	farHomeZoneToMidNeutralGoal.addPoint(SplinePoint(Point(70.3, 75), Vector(30, 0)));
 
-	rightAllianceToRightRings.addPoint(SplinePoint(Point(123.0, 40.0), Vector(15.0, - M_PI_2 - M_PI_4)));
-	rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 46.8), Vector(0.0, 0.0)));
-	rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, 0.0)));
+	int path3 = purePursuit.addPath(farHomeZoneToMidNeutralGoal.getPath(0.01));
 
-	rightAllianceToRightRingsIndex = purePursuit.addPath(rightAllianceToRightRings.getPath(0.1));
+	QuadraticSplinePath midNeutralGoalToPlatform;
 
-	QuadraticSplinePath rightRingsToRightHomeZone = QuadraticSplinePath();
+	midNeutralGoalToPlatform.addPoint(SplinePoint(Point(70.3, 75), Vector(30, 0)));
+	midNeutralGoalToPlatform.addPoint(SplinePoint(Point(75, 107), Vector(30, 0)));
 
-	rightRingsToRightHomeZone.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, M_PI)));
-	rightRingsToRightHomeZone.addPoint(SplinePoint(Point(85, 36), Vector(20.0, M_PI_2)));
+	int path4 = purePursuit.addPath(midNeutralGoalToPlatform.getPath(0.01));
 
-	rightRingsToRightHomeZoneIndex = purePursuit.addPath(rightRingsToRightHomeZone.getPath(0.1));
+	QuadraticSplinePath farPlatformToDropOffGoal;
 
+	farPlatformToDropOffGoal.addPoint(SplinePoint(Point(65, 107), Vector(5, 0)));
+	farPlatformToDropOffGoal.addPoint(SplinePoint(Point(65, 80), Vector(5, 0)));
+
+	int path5 = purePursuit.addPath(farPlatformToDropOffGoal.getPath(0.01));
+
+	QuadraticSplinePath goalDropOffToFarLeftAlliance;
+
+	goalDropOffToFarLeftAlliance.addPoint(SplinePoint(Point(65, 80), Vector(15, 0)));
+	goalDropOffToFarLeftAlliance.addPoint(SplinePoint(Point(18, 97), Vector(15, M_PI_4)));
+
+	int path6 = purePursuit.addPath(goalDropOffToFarLeftAlliance.getPath(0.01));
+
+	QuadraticSplinePath farLeftAllianceToLeftRings;
+
+	farLeftAllianceToLeftRings.addPoint(SplinePoint(Point(18, 97), Vector(15, M_PI_4 + M_PI)));
+	farLeftAllianceToLeftRings.addPoint(SplinePoint(Point(23.2, 70.3), Vector(15, M_PI)));
+
+	int path7 = purePursuit.addPath(farLeftAllianceToLeftRings.getPath(0.01));
+
+	QuadraticSplinePath leftRingsToGoalDrop;
+
+	leftRingsToGoalDrop.addPoint(SplinePoint(Point(23.2, 70.3), Vector(15, 0)));
+	leftRingsToGoalDrop.addPoint(SplinePoint(Point(60, 73), Vector(2, -M_PI_4)));
+	leftRingsToGoalDrop.addPoint(SplinePoint(Point(75, 107), Vector(10, 0)));
+
+	int path8 = purePursuit.addPath(leftRingsToGoalDrop.getPath(0.01));
+
+	QuadraticSplinePath farPlatformToEnterHomezone;
+
+	farPlatformToEnterHomezone.addPoint(SplinePoint(Point(75, 107), Vector(2, 0)));
+	farPlatformToEnterHomezone.addPoint(SplinePoint(Point(75, 95), Vector(2, 0)));
+
+	int path9 = purePursuit.addPath(farPlatformToEnterHomezone.getPath(0.01));
+
+	QuadraticSplinePath enterHomeZoneToRightHomeZone;
+
+	enterHomeZoneToRightHomeZone.addPoint(SplinePoint(Point(75, 95), Vector(10, 0)));
+	enterHomeZoneToRightHomeZone.addPoint(SplinePoint(Point(129.2, 105.7), Vector(10, -M_PI_2)));
+
+	int path10 = purePursuit.addPath(enterHomeZoneToRightHomeZone.getPath(0.01));
+
+	QuadraticSplinePath rightHomeZoneToFarRightAllianceGoal;
+
+	rightHomeZoneToFarRightAllianceGoal.addPoint(SplinePoint(Point(129.2, 105.7), Vector(10, M_PI_2)));
+	rightHomeZoneToFarRightAllianceGoal.addPoint(SplinePoint(Point(103, 125), Vector(10, M_PI_4)));
+
+	int path11 = purePursuit.addPath(rightHomeZoneToFarRightAllianceGoal.getPath(0.01));
+
+	QuadraticSplinePath farRightAllianceGoalToNearPreloads;
+
+	farRightAllianceGoalToNearPreloads.addPoint(SplinePoint(Point(103, 125), Vector(40, M_PI_4)));
+	farRightAllianceGoalToNearPreloads.addPoint(SplinePoint(Point(129.2, 70.3), Vector(20, -M_PI)));
+	farRightAllianceGoalToNearPreloads.addPoint(SplinePoint(Point(117.5, 5), Vector(10, 0)));
+
+	int path12 = purePursuit.addPath(farRightAllianceGoalToNearPreloads.getPath(0.01));
+
+	SplinePath nearPlatformAlign;
+
+	nearPlatformAlign.addPoint(117.5, 5);
+	nearPlatformAlign.addPoint(117.5, 11.4);
+	nearPlatformAlign.addPoint(130, 11.4);
+
+	int path13 = purePursuit.addPath(nearPlatformAlign.getPath(0.01));
+
+	QuadraticSplinePath leftHomeZoneToPlatform;
+
+	leftHomeZoneToPlatform.addPoint(SplinePoint(Point(130, 11.4), Vector(10, -M_PI_2)));
+	leftHomeZoneToPlatform.addPoint(SplinePoint(Point(80, 11.4), Vector(10, -M_PI_2)));
+
+	int path14 = purePursuit.addPath(leftHomeZoneToPlatform.getPath(0.01));
 
 #if GRAPH
 	int gd = DETECT, gm;
@@ -352,7 +423,7 @@ int main() {
 
 	delay(1000);
 #endif
-	Position startingPosition(30.0, 11.4, M_PI_2);
+	Position startingPosition(23.2, 11.4, M_PI_2);
 
 	drivetrain.reset(&startingPosition);
 
diff --git a/runPathTest.sh b/runPathTest.sh
index c1aa1044..5a32ffe2 100755
--- a/runPathTest.sh
+++ b/runPathTest.sh
@@ -1,2 +1,2 @@
-g++ pathTest.cpp -opathTest -lgraph -Iinclude/ -Iinclude/utils/ -Iinclude/chassis/ -Iinclude/odometry/ -Iinclude/motionControl/ -Iinclude/pid/
+g++ pathTest.cpp -opathTest -lgraph -Iinclude/ -Iinclude/utils/ -Iinclude/chassis/ -Iinclude/odometry/ -Iinclude/motionControl/ -Iinclude/feedbackControllers/
 ./pathTest
\ No newline at end of file
diff --git a/src/driver/motorButton.cpp b/src/driver/motorButton.cpp
index 7d138e7f..079d5e5d 100644
--- a/src/driver/motorButton.cpp
+++ b/src/driver/motorButton.cpp
@@ -1,74 +1,83 @@
 #include "motorButton.hpp"
 
-
-
 namespace Pronounce {
-    MotorButton::MotorButton(pros::Controller* controller, pros::Motor* motor,
-            pros::controller_digital_e_t positiveButton,
-            pros::controller_digital_e_t negativeButton,
-            int positiveAuthority,
-            int neutralAuthority,
-            int negativeAuthority,
-            int min,
-            int max) : Button(controller, positiveButton, negativeButton) {
-        this->motor = motor;
-        this->positiveAuthority = positiveAuthority;
-        this->neutralAuthority = neutralAuthority;
-        this->negativeAuthority = negativeAuthority;
-        this->min = min;
-        this->max = max;
-    }
+	MotorButton::MotorButton(pros::Controller* controller, pros::Motor* motor,
+		pros::controller_digital_e_t positiveButton,
+		pros::controller_digital_e_t negativeButton,
+		int positiveAuthority,
+		int neutralAuthority,
+		int negativeAuthority,
+		int min,
+		int max) : Button(controller, positiveButton, negativeButton) {
+		this->motor = motor;
+		this->positiveAuthority = positiveAuthority;
+		this->neutralAuthority = neutralAuthority;
+		this->negativeAuthority = negativeAuthority;
+		this->min = min;
+		this->max = max;
+	}
+
+	void MotorButton::updateActuator() {
+		if (!this->getEnabled()) {
+			this->motor->move_velocity(0.0);
+			return;
+		}
 
-    void MotorButton::updateActuator() {
-        if (!this->getEnabled()) {
-            this->motor->move_velocity(0.0);
-            return;
-        }
+		if (this->getAutonomous() && !this->autonomousPosition) {
+			if (this->goToHeight) {
+				// Use autonomous authority to calculate the angle that the lift needs to be at. 
+				double angle = asin((this->autonomousAuthority - this->height) / this->length) * this->gearRatio;
 
-        if (this->getAutonomous() && !this->autonomousButton) {
-            this->motor->move_absolute(this->autonomousAuthority, abs(this->positiveAuthority));
-            return;
-        }
+				// Set the lift to the angle.
+				this->motor->move_absolute(angle, abs(this->positiveAuthority));
+			}
+			else {
+				this->motor->move_absolute(this->autonomousAuthority, abs(this->positiveAuthority));
+			}
+			return;
+		}
 
-        switch (this->getButtonStatus()) {
-        case NEGATIVE:
-            if (goToImmediately) {
-                this->motor->move_absolute(min, negativeAuthority);
-            }
-            else if (this->motor->get_position() > this->min && this->min < this->max) {
-                this->motor->move(negativeAuthority);
-            } else if (this->min >= this->max) {
-                this->motor->move(negativeAuthority);
-            }
-            else {
-                this->motor->move_velocity(neutralAuthority);
-            }
-            break;
-        case POSITIVE:
-            if (goToImmediately) {
-                this->motor->move_absolute(max, positiveAuthority);
-            }
-            else if (this->motor->get_position() < this->max && this->min < this->max) {
-                this->motor->move(positiveAuthority);
-            }else if (this->min >= this->max) {
-                this->motor->move(positiveAuthority);
-            }
-            else {
-                this->motor->move_velocity(neutralAuthority);
-            }
-            break;
-        case NEUTRAL:
-        default:
-            if (this->getSingleToggle() && goToImmediately) {
-                this->motor->move_absolute(min, neutralAuthority);
-            }
-            if (goToImmediately)
-                return;
-            this->motor->move_velocity(neutralAuthority);
-            break;
-        }
-    }
+		switch (this->getButtonStatus()) {
+		case NEGATIVE:
+			if (goToImmediately) {
+				this->motor->move_absolute(min, negativeAuthority);
+			}
+			else if (this->motor->get_position() > this->min && this->min < this->max) {
+				this->motor->move(negativeAuthority);
+			}
+			else if (this->min >= this->max) {
+				this->motor->move(negativeAuthority);
+			}
+			else {
+				this->motor->move_velocity(neutralAuthority);
+			}
+			break;
+		case POSITIVE:
+			if (goToImmediately) {
+				this->motor->move_absolute(max, positiveAuthority);
+			}
+			else if (this->motor->get_position() < this->max && this->min < this->max) {
+				this->motor->move(positiveAuthority);
+			}
+			else if (this->min >= this->max) {
+				this->motor->move(positiveAuthority);
+			}
+			else {
+				this->motor->move_velocity(neutralAuthority);
+			}
+			break;
+		case NEUTRAL:
+		default:
+			if (this->getSingleToggle() && goToImmediately) {
+				this->motor->move_absolute(min, neutralAuthority);
+			}
+			if (goToImmediately)
+				return;
+			this->motor->move_velocity(neutralAuthority);
+			break;
+		}
+	}
 
-    MotorButton::~MotorButton() {
-    }
+	MotorButton::~MotorButton() {
+	}
 } // namespace Pronounce
diff --git a/src/feedbackControllers/bangBang.cpp b/src/feedbackControllers/bangBang.cpp
new file mode 100644
index 00000000..f060b7dc
--- /dev/null
+++ b/src/feedbackControllers/bangBang.cpp
@@ -0,0 +1,47 @@
+#include "bangBang.hpp"
+
+namespace Pronounce {
+	BangBang::BangBang() : BangBang(0, false, 0) {
+	}
+
+	BangBang::BangBang(double minimumDifference) : BangBang(minimumDifference, false, 0) {
+	}
+
+	BangBang::BangBang(double minimumDifference, bool reversable) : BangBang(minimumDifference, reversable, 0){
+	}
+
+	BangBang::BangBang(double minimumDifference, bool reversable, double outputStrength) {
+		this->minimumDifference = minimumDifference;
+		this->reversable = reversable;
+		this->outputStrength = outputStrength;
+	}
+
+	double BangBang::update(double input) {
+		// Get the difference between the set point and the current value
+		double difference = setPoint - input;
+
+		// Set the lastInput variable to this loop's input.
+		this->lastInput = input;
+		
+		// Change behavior if the controller is not reversable
+		if (reversable) {
+			if (abs(difference) > minimumDifference) {
+				// Return the opisite of the way that we are so we go that way
+				return outputStrength * -signnum_c(difference);
+			} else {
+				// Don't return anything if the value is null.
+				return 0;
+			}
+		} else {
+			if (difference < -minimumDifference) {
+				// Return the opisite of the way that we are so we go that way	
+				return outputStrength * -1;
+			} else {
+				return 0;
+			}
+		}
+	}
+
+	BangBang::~BangBang() {
+	}
+} // namespace Pronounce
diff --git a/src/pid/pid.cpp b/src/feedbackControllers/pid.cpp
similarity index 100%
rename from src/pid/pid.cpp
rename to src/feedbackControllers/pid.cpp
diff --git a/src/main.cpp b/src/main.cpp
index 43ed66ed..65c61053 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -40,6 +40,8 @@ TankDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &midLeftMotor, &mid
 
 Pronounce::TankPurePursuit purePursuit(&drivetrain, &odometry, 20);
 
+Balance balance(&drivetrain, &imu, new BangBang(15, true, 200), new PID(20, 0, 0));
+
 MotorButton liftButton(&master, &lift, DIGITAL_L1, DIGITAL_L2, 200, 0, -200, 0, 0);
 MotorButton intakeButton(&master, &intake, DIGITAL_R2, DIGITAL_Y, 200, 0, -100, 0, 0);
 
@@ -76,6 +78,7 @@ int preAutonRun() {
 	frontGrabberButton.setAutonomous(true);
 	backGrabberButton.setAutonomous(true);
 	liftButton.setAutonomous(true);
+	liftButton.setAutonomousPosition(true);
 	backGrabberButton.setAutonomous(true);
 	intakeButton.setAutonomous(true);
 
@@ -319,6 +322,8 @@ int postAuton() {
 	liftButton.setAutonomous(false);
 	intakeButton.setAutonomous(false);
 
+	balance.setEnabled(false);
+
 	return 0;
 }
 
@@ -387,6 +392,10 @@ void initMotors() {
 	backGrabberButton.setSingleToggle(true);
 
 	intakeButton.setSingleToggle(true);
+	intakeButton.setDejam(true);
+	intakeButton.setDejamAuthority(-100);
+	intakeButton.setDejamSpeed(5);
+	intakeButton.setDejamTime(500);
 
 	lift.set_current_limit(25000);
 
diff --git a/src/motionControl/balance.cpp b/src/motionControl/balance.cpp
new file mode 100644
index 00000000..973c2c29
--- /dev/null
+++ b/src/motionControl/balance.cpp
@@ -0,0 +1,38 @@
+#include "balance.hpp"
+
+namespace Pronounce {
+	Balance::Balance() : Balance(nullptr, nullptr) {
+	}
+
+	Balance::Balance(AbstractTankDrivetrain* drivetrain) : Balance(drivetrain, nullptr) {
+	}
+
+	Balance::Balance(AbstractTankDrivetrain* drivetrain, pros::Imu* imu) : Balance(drivetrain, imu, new BangBang(), new PID()) {
+	}
+
+	Balance::Balance(AbstractTankDrivetrain* drivetrain, pros::Imu* imu, BangBang* linearController, PID* orientationController) {
+		this->drivetrain = drivetrain;
+		this->imu = imu;
+		this->linearController = linearController;
+		this->orientationController = orientationController;
+		orientationController->setTurnPid(true);
+	}
+
+	void Balance::update() {
+		if (imu == nullptr || drivetrain == nullptr)
+			return;
+
+		// Set the speed with the bang bang controller
+		double speed = linearController->update(imu->get_pitch());
+
+		// Calculate the turing force with the orientation PID
+		this->orientationController->setPosition(this->imu->get_heading());
+		double turn = this->orientationController->update();
+
+		// Send the calculated values to the drivetrain
+		this->drivetrain->skidSteerVelocity(speed, turn);
+	}
+
+	Balance::~Balance() {
+	}
+} // namespace Pronounce