diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index fe5f0699..a6c7a5db 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -19,7 +19,7 @@ namespace Pronounce {
 	int leftHomeZoneToLeftNeutralGoalIndex = -1;
 	int leftNeutralGoalToFarHomeZoneIndex = -1;
 	int farLeftHomeZoneToFarRightGoalDropOffIndex = -1;
-	int farRightDropOffToFarLeftAllianceGoalIndex = -1;
+	int farPlatformToFarLeftAllianceGoalIndex = -1;
 	int farLeftAllianceToMidGoalIndex = -1;
 	int midGoalToFarPlatformIndex = -1;
 	int farPlatformToFarLeftAllianceDropOffIndex = -1;
@@ -37,6 +37,13 @@ namespace Pronounce {
 	int farLeftAllianceDropOffToPlatformIndex = -1;
 	int platformToEnterFarLeftHomeZoneIndex = -1;
 	int enterFarLeftHomeZoneToNearRightPlatformIndex = -1;
+	int farPlatformToEnterFarHomeZoneIndex = -1;
+	int enterFarHomeZoneToMidGoalIndex = -1;
+	int farPlatformToRightAllianceDropOffIndex = -1;
+	int rightAllianceToFarPlatformIndex = -1;
+	int rightNeutralGoalToNearRIghtPlatformIndex = -1;
+	int farLeftAllianceGoalToRightNeutralGoalIndex = -1;
+
 
 	// Left Steal
 	int leftHomeZoneToLeftNeutralStealIndex = -1;
@@ -45,6 +52,7 @@ namespace Pronounce {
 	// Right steal
 	int rightHomeZoneToRightNeutralIndex = -1;
 	int rightNeutralToRightHomeZoneIndex = -1;
+	int rightHomeZoneToRightNeutralClawIndex = -1;
 
 	// Right(Right position) steal
 	int farRightHomeZoneToRightNeutralIndex = -1;
@@ -58,11 +66,11 @@ namespace Pronounce {
 	int leftHomeZoneToLeftAllianceIndex = -1;
 	int leftAllianceToRightHomeZoneIndex = -1;
 	int rightHomeZoneToLeftAllianceIndex = -1;
-	int leftAllianceToPreloadsIndex = -1;
-	int preloadsToLeftAllianceIndex = -1;
+	int rightHomeZoneToLeftHomeZoneIndex = -1;
 
 	// Right AWP
 	int enterRightHomeZoneToRightAllianceIndex = -1;
+	int rightPlatformToRightAllianceIndex = -1;
 	int rightAllianceToRightRingsIndex = -1;
 	int rightAllianceToLeftAllianceIndex = -1;
 	int rightRingsToRightHomeZoneIndex = -1;
@@ -94,318 +102,320 @@ namespace Pronounce {
 
 		// !SECTION
 
-		//SECTION Skills
+		// SECTION Left Steal
 
-		// Left home zone to left neutral goal
-		QuadraticSplinePath leftHomeZoneToLeftNeutralGoal("Left Home Zone to Left Neutral Goal");
+		Path leftHomeZoneToLeftNeutralSteal;
+		double leftDistance = 25;
+		double leftAngle = toRadians(10);
+		// 25 in
+		leftHomeZoneToLeftNeutralSteal.addPoint(27, 18);
+		leftHomeZoneToLeftNeutralSteal.addPoint(27.0+sin(leftAngle) * leftDistance, 18.0 + cos(leftAngle)*leftDistance);
 
-		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(26.5, 11), Vector(20, M_PI_2)));
-		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(38, 67), Vector(10, 0)));
+		leftHomeZoneToLeftNeutralStealIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralSteal);
 
-		leftHomeZoneToLeftNeutralGoalIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.05));
+		Path leftNeutralStealToLeftHomeZone;
 
-		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
+		leftNeutralStealToLeftHomeZone.addPoint(27+sin(leftAngle) * leftDistance, 18 + cos(leftAngle)*leftDistance);
+		leftNeutralStealToLeftHomeZone.addPoint(25, 18);
 
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 72), Vector(25, -M_PI_4)));
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(73, 110), Vector(30, 0.1)));
+		leftNeutralStealToLeftHomeZoneIndex = purePursuit->addPath(leftNeutralStealToLeftHomeZone);
 
-		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
+		// !SECTION
 
-		QuadraticSplinePath farLeftHomeZoneToFarRightGoalDropOff("Left Home Zone to far right goal drop off");
+		// SECTION Right steal
 
-		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(60, 107), Vector(0.0, 0.0)));
-		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(85, 107), Vector(0.0, 0.0)));
+		QuadraticSplinePath rightHomeZoneToRightNeutral;
 
-		farLeftHomeZoneToFarRightGoalDropOffIndex = purePursuit->addPath(farLeftHomeZoneToFarRightGoalDropOff.getPath(0.05));
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 43), Vector(0.0, 0)));
 
-		QuadraticSplinePath farRightDropOffToFarLeftAllianceGoal;
+		rightHomeZoneToRightNeutralIndex = purePursuit->addPath(rightHomeZoneToRightNeutral.getPath(0.1));
 
-		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(82, 102), Vector(20.0, M_PI_2)));
-		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(20, 105), Vector(20.0, M_PI_2)));
+		QuadraticSplinePath rightNeutralToRightHomeZone;
 
-		farRightDropOffToFarLeftAllianceGoalIndex = purePursuit->addPath(farRightDropOffToFarLeftAllianceGoal.getPath(0.05));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 55), Vector(0.0, 0)));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 17), Vector(0.0, 0)));
 
-		QuadraticSplinePath farLeftAllianceToMidGoal("farRightDropOffToFarLeftAllianceGoal");
+		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.1));
 
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(15, 105.7), Vector(5, -M_PI_2)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(25, 85), Vector(15, M_PI)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(40, 75), Vector(10.0, -M_PI_2)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(72, 70), Vector(5.0, -M_PI_2)));
+		QuadraticSplinePath rightHomeZoneToRightNeutralClaw;
 
-		farLeftAllianceToMidGoalIndex = purePursuit->addPath(farLeftAllianceToMidGoal.getPath(0.05));
+		rightHomeZoneToRightNeutralClaw.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutralClaw.addPoint(SplinePoint(Point(107, 55), Vector(0.0, 0)));
 
-		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
+		rightHomeZoneToRightNeutralClawIndex = purePursuit->addPath(rightHomeZoneToRightNeutralClaw.getPath(0.1));
 
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(60, 70), Vector(0.0, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(75, 105), Vector(0.0, 0)));
+		// !SECTION
 
-		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.05));
+		// SECTION Right right steal
 
-		QuadraticSplinePath farPlatformToFarLeftAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
+		QuadraticSplinePath farRightHomeZoneToRightNeutral;
 
-		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(85, 105.7), Vector(0.0, 0)));
-		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(75, 105.7), Vector(0.0, 0)));
+		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
+		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
 
-		farPlatformToFarLeftAllianceDropOffIndex = purePursuit->addPath(farPlatformToFarLeftAllianceDropOff.getPath(0.05));
+		farRightHomeZoneToRightNeutralIndex = purePursuit->addPath(farRightHomeZoneToRightNeutral.getPath(0.1));
 
-		QuadraticSplinePath farLeftAllianceDropOffToFarRightDropOff("farPlatformToFarLeftAllianceDropOff");
+		QuadraticSplinePath rightNeutralToFarRightHomeZone;
 
-		farLeftAllianceDropOffToFarRightDropOff.addPoint(SplinePoint(Point(58.9, 105.7), Vector(7.0, -M_PI_2)));
-		farLeftAllianceDropOffToFarRightDropOff.addPoint(SplinePoint(Point(92, 105), Vector(7.0, -M_PI_2)));
+		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
+		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
 
-		farLeftAllianceDropOffToFarRightDropOffIndex = purePursuit->addPath(farLeftAllianceDropOffToFarRightDropOff.getPath(0.05));
-		
-		QuadraticSplinePath farRightDropOffToPlatform("farRightDropOffToPlatform");
+		rightNeutralToFarRightHomeZoneIndex = purePursuit->addPath(rightNeutralToFarRightHomeZone.getPath(0.1));
 
-		farRightDropOffToPlatform.addPoint(SplinePoint(Point(92, 105.7), Vector(7.0, 0)));
-		farRightDropOffToPlatform.addPoint(SplinePoint(Point(92, 110), Vector(7.0, 0)));
+		// SECTION Mid Steal
 
-		farRightDropOffToPlatformIndex = purePursuit->addPath(farRightDropOffToPlatform.getPath(0.05));
+		double midGoalDistance = 32.66;
+		double angle = -30;
+		
+		Path rightHomeZoneToMidNeutral;
+		// Distance: 32
+		rightHomeZoneToMidNeutral.addPoint(99.5, 18);
+		rightHomeZoneToMidNeutral.addPoint(99.5+(sin(toRadians(angle)) * midGoalDistance), 18+(cos(toRadians(angle)) * midGoalDistance));
 
-		QuadraticSplinePath platformToEnterFarRightHomeZone("platformToEnterFarRightHomeZone");
+		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral);
 
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 115), Vector(7.0, M_PI)));
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 105.7), Vector(7.0, M_PI)));
+		Path midNeutralToRightHomeZone;
 
-		platformToEnterFarRightHomeZoneIndex = purePursuit->addPath(platformToEnterFarRightHomeZone.getPath(0.05));
+		midNeutralToRightHomeZone.addPoint(99.5+(sin(toRadians(angle)) * midGoalDistance), 18+(cos(toRadians(angle)) * midGoalDistance));
+		midNeutralToRightHomeZone.addPoint(105, 35);
+		midNeutralToRightHomeZone.addPoint(105, 25);
 
-		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
+		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone);
 
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 105.7), Vector(20.0, -M_PI_2)));
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(105.7, 125), Vector(40.0, M_PI_4 * 1.5)));
+		//!SECTION
+			
+		// SECTION Left AWP
+		
+		QuadraticSplinePath leftHomeZoneToLeftAlliance;
+		
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 30), Vector(10.0, M_PI)));
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(37, 17), Vector(20, M_PI + M_PI_4)));
 
-		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.05));
+		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.1));
 
-		QuadraticSplinePath farRightAllianceToRightHomeZone("farRightAllianceToRightHomeZone");
+		QuadraticSplinePath leftAllianceToRightHomeZone;
+		
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI_4)));
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(46.8, 42), Vector(20.0, -M_PI_2)));
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(98, 40.0), Vector(10.0, -M_PI_2)));
 
-		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(105.7, 130), Vector(20.0, M_PI_4 * 1.3 + M_PI)));
-		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(135, 70), Vector(20.0, M_PI)));
-		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(125, 30), Vector(20.0, M_PI)));
+		leftAllianceToRightHomeZoneIndex = purePursuit->addPath(leftAllianceToRightHomeZone.getPath(0.1));
 
-		farRightAllianceToRightHomeZoneIndex = purePursuit->addPath(farRightAllianceToRightHomeZone.getPath(0.05));
+		QuadraticSplinePath rightHomeZoneToLeftHomeZone;
 
-		QuadraticSplinePath rightHomeZoneToRightAlliance("rightHomeZoneToRightAlliance");
+		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(110, 32), Vector(0.0, M_PI_2)));
+		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(25, 30), Vector(0.0, M_PI_2)));
 
-		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(105.7, 15), Vector(5.0, -M_PI_4)));
-		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(145, 35), Vector(10.0, -M_PI_4 * 0.6)));
+		rightHomeZoneToLeftHomeZoneIndex = purePursuit->addPath(rightHomeZoneToLeftHomeZone.getPath(0.2));
 
-		rightHomeZoneToRightAllianceIndex = purePursuit->addPath(rightHomeZoneToRightAlliance.getPath(0.05));
+		QuadraticSplinePath rightHomeZoneToLeftAlliance;
 
-		QuadraticSplinePath rightAllianceToRightNeutral("rightAllianceToRightNeutral");
+		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 34), Vector(10, M_PI)));
+		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(38, 15), Vector(20, M_PI + (M_PI_2*1.3))));
+		
+		rightHomeZoneToLeftAllianceIndex = purePursuit->addPath(rightHomeZoneToLeftAlliance.getPath(0.2));
 
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(125, 30), Vector(10.0, 0)));
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(110, 62), Vector(5.0, M_PI_4)));
+		// !SECTION
 
-		rightAllianceToRightNeutralIndex = purePursuit->addPath(rightAllianceToRightNeutral.getPath(0.05));
+		// SECTION Right AWP
 
-		QuadraticSplinePath rightNeutralToPlatform("rightNeutralToPlatform");
+		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
+		
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(125, 32), Vector(15, -M_PI_2)));
 
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(110, 62), Vector(15.0, 0)));
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
+		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.1));
 
-		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.05));
+		QuadraticSplinePath rightPlatformToRightAlliance;
+		
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(125, 35), Vector(15, -M_PI_2)));
 
-		QuadraticSplinePath rightPlatformToFarRightGoalDropOff2("rightPlatformToFarRightGoalDropOff2");
+		rightPlatformToRightAllianceIndex = purePursuit->addPath(rightPlatformToRightAlliance.getPath(0.1));
 
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 115), Vector(15.0, M_PI)));
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI)));
+		QuadraticSplinePath rightAllianceToRightRings;
 
-		rightPlatformToFarRightGoalDropOff2Index = purePursuit->addPath(rightPlatformToFarRightGoalDropOff2.getPath(0.05));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(127, 37), Vector(10, 0)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(122, 55), Vector(10.0, 0)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(130, 65), Vector(10.0, -M_PI_2)));
 
-		QuadraticSplinePath farRightGoalDropOff2ToFarLeftAllianceDropOff("farRightGoalDropOff2ToFarLeftAllianceDropOff");
+		rightAllianceToRightRingsIndex = purePursuit->addPath(rightAllianceToRightRings.getPath(0.1));
 
-		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI_2)));
-		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(60, 105.7), Vector(15.0, M_PI_2)));
+		QuadraticSplinePath rightRingsToRightHomeZone;
 
-		farRightGoalDropOff2ToFarLeftAllianceDropOffIndex = purePursuit->addPath(farRightGoalDropOff2ToFarLeftAllianceDropOff.getPath(0.05));
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(40, M_PI_2*0.9)));
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(110, 30), Vector(15, M_PI)));
 
-		QuadraticSplinePath farLeftAllianceDropOffToFarRightGoalDropOff2("farLeftAllianceDropOffToFarRightGoalDropOff2");
+		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.1));
 
-		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(60, 105.7), Vector(15.0, -M_PI_2)));
-		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, -M_PI_2)));
+		// !SECTION
 
-		farLeftAllianceDropOffToFarRightGoalDropOff2Index = purePursuit->addPath(farLeftAllianceDropOffToFarRightGoalDropOff2.getPath(0.05));
 
-		QuadraticSplinePath farLeftAllianceDropOffToPlatform("farLeftAllianceDropOffToPlatform");
+		//SECTION Skills
 
-		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(60, 105.7), Vector(15.0, 0)));
-		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(60, 115), Vector(15.0, 0)));
+		// Left home zone to left neutral goal
+		QuadraticSplinePath leftHomeZoneToLeftNeutralGoal("Left Home Zone to Left Neutral Goal");
 
-		farLeftAllianceDropOffToPlatformIndex = purePursuit->addPath(farLeftAllianceDropOffToPlatform.getPath(0.05));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(26.5, 11), Vector(15, M_PI_2)));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(35, 67), Vector(10, 0)));
 
-		QuadraticSplinePath platformToEnterFarLeftHomeZone("platformToEnterFarLeftHomeZone");
+		leftHomeZoneToLeftNeutralGoalIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.1));
 
-		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(60, 115), Vector(15.0, M_PI)));
-		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(60, 105.7), Vector(15.0, M_PI)));
+		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
 
-		platformToEnterFarLeftHomeZoneIndex = purePursuit->addPath(platformToEnterFarLeftHomeZone.getPath(0.05));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 67), Vector(15, -M_PI_4)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(70, 112), Vector(25, 0.1)));
 
-		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
+		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
 
-		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(60, 105.7), Vector(25, -M_PI_2)));
-		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(85, 70), Vector(25, -M_PI_2)));
-		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(105.7, 45), Vector(10, M_PI*1.2)));
-		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(100, 14), Vector(25, M_PI_2)));
+		QuadraticSplinePath farPlatformToEnterFarHomeZone("Left Home Zone to far right goal drop off");
 
-		enterFarLeftHomeZoneToNearRightPlatformIndex = purePursuit->addPath(enterFarLeftHomeZoneToNearRightPlatform.getPath(0.05));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(70, 112), Vector(0.0, 0.0)));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(70, 95), Vector(0.0, 0.0)));
 
-		// !SECTION
+		farPlatformToEnterFarHomeZoneIndex = purePursuit->addPath(farPlatformToEnterFarHomeZone.getPath(0.1));
 
-		// SECTION Left Steal
+		QuadraticSplinePath enterFarHomeZoneToMidGoal("farLeftAllianceToMidGoal");
 
-		Path leftHomeZoneToLeftNeutralSteal;
+		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(70, 95), Vector(5.0, -M_PI*0.8)));
+		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(70, 70), Vector(10, -M_PI)));
 
-		leftHomeZoneToLeftNeutralSteal.addPoint(27, 18);
-		leftHomeZoneToLeftNeutralSteal.addPoint(33, 48);
+		enterFarHomeZoneToMidGoalIndex = purePursuit->addPath(enterFarHomeZoneToMidGoal.getPath(0.2));
 
-		leftHomeZoneToLeftNeutralStealIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralSteal);
+		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
 
-		Path leftNeutralStealToLeftHomeZone;
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 115), Vector(0.0, 0)));
 
-		leftNeutralStealToLeftHomeZone.addPoint(33, 48);
-		leftNeutralStealToLeftHomeZone.addPoint(25, 18);
+		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.2));
 
-		leftNeutralStealToLeftHomeZoneIndex = purePursuit->addPath(leftNeutralStealToLeftHomeZone);
+		QuadraticSplinePath farPlatformToRightAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
 
-		// !SECTION
+		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 107), Vector(0.0, 0)));
+		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 80), Vector(0.0, 0)));
 
-		// SECTION Right steal
+		farPlatformToRightAllianceDropOffIndex = purePursuit->addPath(farPlatformToRightAllianceDropOff.getPath(0.1));
+		
+		QuadraticSplinePath rightAllianceToFarPlatform("farRightDropOffToPlatform");
 
-		QuadraticSplinePath rightHomeZoneToRightNeutral;
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(80, 80), Vector(7.0, 0)));
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(77, 123), Vector(7.0, 0)));
 
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(105.7, 19.5), Vector(0.0, 0)));
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(105.7, 46), Vector(0.0, 0)));
+		rightAllianceToFarPlatformIndex = purePursuit->addPath(rightAllianceToFarPlatform.getPath(0.1));
 
-		rightHomeZoneToRightNeutralIndex = purePursuit->addPath(rightHomeZoneToRightNeutral.getPath(0.01));
+		QuadraticSplinePath platformToEnterFarRightHomeZone("platformToEnterFarRightHomeZone");
 
-		QuadraticSplinePath rightNeutralToRightHomeZone;
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(80, 115), Vector(7.0, M_PI)));
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(80, 110), Vector(7.0, M_PI)));
 
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(105.7, 46), Vector(0.0, 0)));
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(105.7, 17), Vector(0.0, 0)));
+		platformToEnterFarRightHomeZoneIndex = purePursuit->addPath(platformToEnterFarRightHomeZone.getPath(0.1));
 
-		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.01));
+		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
 
-		// !SECTION
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(80, 100), Vector(20.0, -M_PI_2)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(100, 100), Vector(20.0, -M_PI_2)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(110, 127), Vector(20.0, M_PI_2*1.1)));
 
-		// SECTION Right right steal
+		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.1));
 
-		QuadraticSplinePath farRightHomeZoneToRightNeutral;
+		QuadraticSplinePath farRightAllianceToRightHomeZone("farRightAllianceToRightHomeZone");
 
-		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(117, 19.5), Vector(0.0, 0)));
-		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(112, 46), Vector(0.0, 0)));
+		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(100, 127), Vector(20.0, M_PI_4 * 1.3 + M_PI)));
+		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(125, 70), Vector(20.0, M_PI)));
+		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(115, 20), Vector(20.0, M_PI)));
 
-		farRightHomeZoneToRightNeutralIndex = purePursuit->addPath(farRightHomeZoneToRightNeutral.getPath(0.01));
+		farRightAllianceToRightHomeZoneIndex = purePursuit->addPath(farRightAllianceToRightHomeZone.getPath(0.1));
 
-		QuadraticSplinePath rightNeutralToFarRightHomeZone;
+		QuadraticSplinePath rightHomeZoneToRightAlliance("rightHomeZoneToRightAlliance");
 
-		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(112, 46), Vector(0.0, 0)));
-		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(117, 19.5), Vector(0.0, 0)));
+		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(105.7, 15), Vector(5.0, -M_PI_4)));
+		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(135, 35), Vector(10.0, -M_PI_4 * 0.6)));
 
-		rightNeutralToFarRightHomeZoneIndex = purePursuit->addPath(rightNeutralToFarRightHomeZone.getPath(0.01));
+		rightHomeZoneToRightAllianceIndex = purePursuit->addPath(rightHomeZoneToRightAlliance.getPath(0.1));
 
-		// SECTION Mid Steal
+		QuadraticSplinePath rightAllianceToRightNeutral("rightAllianceToRightNeutral");
 
-		QuadraticSplinePath rightHomeZoneToMidNeutral;
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(145, 35), Vector(10.0, 0)));
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(122, 62), Vector(5.0, M_PI_4)));
 
-		rightHomeZoneToMidNeutral.addPoint(SplinePoint(Point(100, 16), Vector(0.0, 0)));
-		rightHomeZoneToMidNeutral.addPoint(SplinePoint(Point(83, 48), Vector(0.0, 0)));
+		rightAllianceToRightNeutralIndex = purePursuit->addPath(rightAllianceToRightNeutral.getPath(0.1));
 
-		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral.getPath(0.01));
+		QuadraticSplinePath rightNeutralToPlatform("rightNeutralToPlatform");
 
-		QuadraticSplinePath midNeutralToRightHomeZone;
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(122, 62), Vector(15.0, 0)));
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(100, 115), Vector(15.0, 0)));
 
-		midNeutralToRightHomeZone.addPoint(SplinePoint(Point(83, 48), Vector(0.0, 0)));
-		midNeutralToRightHomeZone.addPoint(SplinePoint(Point(100, 16), Vector(0.0, 0)));
+		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.1));
 
-		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone.getPath(0.01));
+		QuadraticSplinePath rightPlatformToFarRightGoalDropOff2("rightPlatformToFarRightGoalDropOff2");
 
-		//!SECTION
-			
-		// SECTION Left AWP
-		
-		QuadraticSplinePath leftHomeZoneToLeftAlliance;
-		
-		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 30), Vector(10.0, M_PI)));
-		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI + M_PI_4)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(100, 115), Vector(15.0, M_PI)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 100), Vector(15.0, M_PI)));
 
-		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.01));
+		rightPlatformToFarRightGoalDropOff2Index = purePursuit->addPath(rightPlatformToFarRightGoalDropOff2.getPath(0.1));
 
-		QuadraticSplinePath leftAllianceToRightHomeZone;
-		
-		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI_4)));
-		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(46.8, 46.8), Vector(20.0, -M_PI_2)));
-		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
+		QuadraticSplinePath farRightGoalDropOff2ToFarLeftAllianceDropOff("farRightGoalDropOff2ToFarLeftAllianceDropOff");
 
-		leftAllianceToRightHomeZoneIndex = purePursuit->addPath(leftAllianceToRightHomeZone.getPath(0.01));
+		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI_2)));
+		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(25, 105.7), Vector(15.0, M_PI_2)));
 
-		QuadraticSplinePath rightHomeZoneToLeftAlliance;
+		farRightGoalDropOff2ToFarLeftAllianceDropOffIndex = purePursuit->addPath(farRightGoalDropOff2ToFarLeftAllianceDropOff.getPath(0.1));
 
-		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(98, 35), Vector(20.0, M_PI_2)));
-		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(46, 35), Vector(10.0, M_PI_2)));
-		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(30, 17), Vector(10.0, M_PI + M_PI_4)));
-		
-		rightHomeZoneToLeftAllianceIndex = purePursuit->addPath(rightHomeZoneToLeftAlliance.getPath(0.01));
+		QuadraticSplinePath farLeftAllianceDropOffToFarRightGoalDropOff2("farLeftAllianceDropOffToFarRightGoalDropOff2");
 
-		QuadraticSplinePath leftAllianceToPreloads;
+		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(25, 105.7), Vector(15.0, -M_PI_2)));
+		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, -M_PI_2)));
 
-		leftAllianceToPreloads.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI_2)));
-		leftAllianceToPreloads.addPoint(SplinePoint(Point(15, 15), Vector(10.0, M_PI_2)));
+		farLeftAllianceDropOffToFarRightGoalDropOff2Index = purePursuit->addPath(farLeftAllianceDropOffToFarRightGoalDropOff2.getPath(0.1));
 
-		leftAllianceToPreloadsIndex = purePursuit->addPath(leftAllianceToPreloads.getPath(0.01));
+		QuadraticSplinePath farLeftAllianceDropOffToPlatform("farLeftAllianceDropOffToPlatform");
 
-		QuadraticSplinePath preloadsToLeftAlliance;
+		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, 0)));
+		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
 
-		preloadsToLeftAlliance.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI_2)));
-		preloadsToLeftAlliance.addPoint(SplinePoint(Point(15, 15), Vector(10.0, M_PI_2)));
+		farLeftAllianceDropOffToPlatformIndex = purePursuit->addPath(farLeftAllianceDropOffToPlatform.getPath(0.1));
 
-		preloadsToLeftAllianceIndex = purePursuit->addPath(preloadsToLeftAlliance.getPath(0.01));
+		QuadraticSplinePath platformToEnterFarLeftHomeZone("platformToEnterFarLeftHomeZone");
 
-		// !SECTION
+		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 115), Vector(15.0, M_PI)));
+		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI)));
 
-		// SECTION Right AWP
+		platformToEnterFarLeftHomeZoneIndex = purePursuit->addPath(platformToEnterFarLeftHomeZone.getPath(0.1));
 
-		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
-		
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(120, 39), Vector(15, - M_PI_2 - 0.4)));
+		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.01));
+		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(100, 105.7), Vector(25, -M_PI_2)));
+		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(135, 45), Vector(10, M_PI*1.2)));
+		enterFarLeftHomeZoneToNearRightPlatform.addPoint(SplinePoint(Point(132, 14), Vector(25, M_PI_2)));
 
-		QuadraticSplinePath rightAllianceToRightRings;
+		enterFarLeftHomeZoneToNearRightPlatformIndex = purePursuit->addPath(enterFarLeftHomeZoneToNearRightPlatform.getPath(0.1));
 
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 37), Vector(5, M_PI_4)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 55), Vector(10.0, 0)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 60), Vector(10.0, 0)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 70), Vector(10.0, -M_PI_2)));
+		QuadraticSplinePath farPlatformToFarLeftAllianceGoal("enterFarLeftHomeZoneToNearRightPlatform");
 
-		rightAllianceToRightRingsIndex = purePursuit->addPath(rightAllianceToRightRings.getPath(0.01));
+		farPlatformToFarLeftAllianceGoal.addPoint(SplinePoint(Point(80, 110), Vector(25, M_PI_2)));
+		farPlatformToFarLeftAllianceGoal.addPoint(SplinePoint(Point(28, 127), Vector(25, M_PI_2)));
 
-		QuadraticSplinePath rightRingsToRightHomeZone;
+		farPlatformToFarLeftAllianceGoalIndex = purePursuit->addPath(farPlatformToFarLeftAllianceGoal.getPath(0.1));
 
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(20.0, M_PI_2)));
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(135, 25), Vector(15, M_PI)));
+		QuadraticSplinePath farLeftAllianceGoalToRightNeutralGoal("enterFarLeftHomeZoneToNearRightPlatform");
 
-		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.01));
+		farLeftAllianceGoalToRightNeutralGoal.addPoint(SplinePoint(Point(32, 133), Vector(25, -M_PI_2)));
+		farLeftAllianceGoalToRightNeutralGoal.addPoint(SplinePoint(Point(105.7, 70), Vector(40, M_PI)));
 
-		QuadraticSplinePath rightAllianceToLeftAlliance;
-		
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(120, 37), Vector(10.0, M_PI - M_PI_2 - 0.4)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, M_PI_2)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(46.8, 46.8), Vector(10.0, M_PI_2)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI + M_PI_4)));
+		farLeftAllianceGoalToRightNeutralGoalIndex = purePursuit->addPath(farLeftAllianceGoalToRightNeutralGoal.getPath(0.1));
 
-		rightAllianceToLeftAllianceIndex = purePursuit->addPath(rightAllianceToLeftAlliance.getPath(0.01));
+		QuadraticSplinePath rightNeutralGoalToNearRIghtPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		// !SECTION
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(105.7, 80), Vector(25, M_PI)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(80, 60), Vector(20, M_PI_2)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(25, 60), Vector(20, M_PI_2)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(25, 36), Vector(20, -M_PI_2*0.8)));
 
-		QuadraticSplinePath wiggle;
+		rightNeutralGoalToNearRIghtPlatformIndex = purePursuit->addPath(rightNeutralGoalToNearRIghtPlatform.getPath(0.1));
 
-		wiggle.addPoint(SplinePoint(Point(20, 0), Vector(10.0, 0.0)));
-		wiggle.addPoint(SplinePoint(Point(10, 20), Vector(10.0, 0.0)));
-		wiggle.addPoint(SplinePoint(Point(20, 40), Vector(10.0, 0.0)));
-		wiggle.addPoint(SplinePoint(Point(10, 60), Vector(10.0, 0.0)));
+		// !SECTION
 
-		wiggleIndex = purePursuit->addPath(wiggle.getPath(0.01));
 
 		printf("Array size: %d\n", purePursuit->getPaths().size());
 	}
diff --git a/include/chassis/abstractTankDrivetrain.hpp b/include/chassis/abstractTankDrivetrain.hpp
index b3bb936f..62522d0d 100644
--- a/include/chassis/abstractTankDrivetrain.hpp
+++ b/include/chassis/abstractTankDrivetrain.hpp
@@ -37,10 +37,27 @@ namespace Pronounce {
 			this->tankSteerVelocity(leftSpeed, rightSpeed);
 		}
 
+		void driveCurvatureVoltage(double speed, double curvature) {			
+			double leftSpeed = speed * (2.0 + curvature * trackWidth) / 2.0;
+			double rightSpeed = speed * (2.0 - curvature * trackWidth) / 2.0;
+
+			double maxSpeed = max(abs(leftSpeed), abs(rightSpeed));
+
+			if (maxSpeed > abs(speed)) {
+				double multiplier = abs(speed) / maxSpeed;
+				leftSpeed *= multiplier;
+				rightSpeed *= multiplier;
+			}
+
+			this->tankSteerVoltage(leftSpeed, rightSpeed);
+		}
+
 		virtual void skidSteerVelocity(double speed, double turn) {}
 
 		virtual void tankSteerVelocity(double leftSpeed, double rightSpeed) {}
 
+		virtual void tankSteerVoltage(double leftSpeed, double rightSpeed) {}
+
 		~AbstractTankDrivetrain();
 	};
 } // namespace Pronounce
diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp
index 3781e60e..8bd286e0 100644
--- a/include/chassis/tankDrive.hpp
+++ b/include/chassis/tankDrive.hpp
@@ -24,11 +24,21 @@ namespace Pronounce {
 			this->getRightMotors().move_velocity(speed - turn);
 		}
 
+		void skidSteerVoltage(double speed, double turn) {
+			this->getLeftMotors().move_voltage(speed + turn);
+			this->getRightMotors().move_voltage(speed - turn);
+		}
+
 		void tankSteerVelocity(double leftSpeed, double rightSpeed) {
 			this->getLeftMotors().move_velocity(leftSpeed);
 			this->getRightMotors().move_velocity(rightSpeed);
 		}
 
+		void tankSteerVoltage(double leftSpeed, double rightSpeed) {
+			this->getLeftMotors().move_voltage(leftSpeed);
+			this->getRightMotors().move_voltage(rightSpeed);
+		}
+
 		~TankDrivetrain();
 	};
 } // namespace Pronounce
diff --git a/include/feedbackControllers/bangBang.hpp b/include/feedbackControllers/bangBang.hpp
index 1863662a..e0357416 100644
--- a/include/feedbackControllers/bangBang.hpp
+++ b/include/feedbackControllers/bangBang.hpp
@@ -24,6 +24,10 @@ namespace Pronounce {
 		 */
 		double update(double input);
 
+		double getLastInput() {
+			return lastInput;
+		}
+
 		double getSetPoint() {
 			return setPoint;
 		}
diff --git a/include/motionControl/purePursuit.hpp b/include/motionControl/purePursuit.hpp
index 3b989a37..79e3bac5 100644
--- a/include/motionControl/purePursuit.hpp
+++ b/include/motionControl/purePursuit.hpp
@@ -24,6 +24,7 @@ namespace Pronounce {
 		Vector localLookaheadVector;
 		Vector normalizedLookaheadVector;
 		double curvature;
+		double distanceFromEnd;
 	};
 
 	/**
diff --git a/include/motionControl/tankPurePursuit.hpp b/include/motionControl/tankPurePursuit.hpp
index 887b3cfc..08ca18bb 100644
--- a/include/motionControl/tankPurePursuit.hpp
+++ b/include/motionControl/tankPurePursuit.hpp
@@ -17,6 +17,8 @@ namespace Pronounce {
 		PID* turnPid;
 
 		double lastUpdateTime = 0;
+
+		bool useVoltage = false;
 	public:
 		TankPurePursuit(AbstractTankDrivetrain* drivetrain);
 		TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance);
@@ -27,6 +29,14 @@ namespace Pronounce {
 
 		void stop();
 
+		bool getUseVoltage() {
+			return useVoltage;
+		}
+
+		void setUseVoltage(bool useVoltage) {
+			this->useVoltage = useVoltage;
+		}
+
 		AbstractTankDrivetrain* getDrivetrain() {
 			return drivetrain;
 		}
diff --git a/pathTest.cpp b/pathTest.cpp
index 028bcbbe..7a82995e 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,12 +276,14 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
+		QuadraticSplinePath rightNeutralGoalToNearRIghtPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(60, 70), Vector(20, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(75, 70), Vector(20, 0)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(105.7, 80), Vector(25, M_PI)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(80, 60), Vector(20, M_PI_2)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(25, 60), Vector(20, M_PI_2)));
+		rightNeutralGoalToNearRIghtPlatform.addPoint(SplinePoint(Point(25, 36), Vector(20, -M_PI_2*0.8)));
 
-	smoothPath = midGoalToFarPlatform.getPath(0.1);
+	smoothPath = rightNeutralGoalToNearRIghtPlatform.getPath(0.1);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index 87a5b214..5c0b4eaf 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,8 +22,7 @@ pros::ADIDigitalOut frontGrabber(1, false);
 pros::ADIDigitalOut backGrabber(2, false);
 pros::ADIDigitalOut backTilter(3, false);
 pros::ADIDigitalOut frontHook(4, false);
-pros::ADIDigitalIn frontGrabberBumperSwitch(5);
-pros::ADIDigitalIn backGrabberBumperSwitch(6);
+pros::ADIDigitalOut brakes(5, false);
 
 // Inertial Measurement Unit
 pros::Imu imu(5);
@@ -43,7 +42,7 @@ GpsOdometry gpsOdometry(&gps);
 // ThreeWheelOdom odometry(&leftOdomWheel, &rightOdomWheel, &backOdomWheel, &imu);
 ThreeWheelOdom odometry(&leftOdomWheel, &rightOdomWheel, &nullOdomWheel, &imu);
 
-TankDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &midLeftMotor, &midRightMotor, &backLeftMotor, &backRightMotor, &imu, 10.0);
+TankDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &midLeftMotor, &midRightMotor, &backLeftMotor, &backRightMotor, &imu, 8.0);
 
 Pronounce::TankPurePursuit purePursuit(&drivetrain, &odometry, new PID(0.6, 0, 2.0), 20);
 
@@ -56,10 +55,16 @@ SolenoidButton frontGrabberButton(&master, DIGITAL_A);
 SolenoidButton frontHookButton(&master, DIGITAL_X);
 SolenoidButton backGrabberButton(&master, DIGITAL_R1, DIGITAL_R1);
 SolenoidButton backGrabberButton2(&master, DIGITAL_R1, DIGITAL_R1);
+SolenoidButton brakesButton(&master, DIGITAL_B);
+
+pros::Vision clawVision(18, VISION_ZERO_CENTER);
 
 // Autonomous Selector
 Pronounce::AutonSelector autonomousSelector;
 
+// LVGL
+lv_obj_t* tabview;
+
 #define DRIFT_MIN 7.0
 
 bool preDriverTasksDone = false;
@@ -68,6 +73,7 @@ bool disableIntake = true;
 int driverMode = 0;
 
 bool backButtonStatus = false;
+bool backGrabberManual = false;
 uint32_t lastChange = 0;
 
 // SECTION Auton
@@ -80,6 +86,18 @@ void backGrabberChange(bool backButtonStatus2) {
 	lastChange = pros::millis();
 }
 
+double goalAngle() {
+	if (clawVision.get_object_count() < 1) {
+		return 0.0;
+	}
+
+	int x = clawVision.get_by_sig(0, 1).x_middle_coord;
+
+	double angle = 0.0033908 * (double)x + 0.064622;
+
+	return angle;
+}
+
 void waitForDone(double distance, double timeout) {
 	uint32_t startTime = pros::millis();
 
@@ -92,7 +110,7 @@ void waitForDone(double distance, double timeout) {
 void waitForDone(double distance) {
 	// Wait for done
 	while (!purePursuit.isDone(distance)) {
-		pros::Task::delay(30);
+		pros::Task::delay(20);
 	}
 }
 
@@ -107,7 +125,7 @@ void waitForDoneOrientation() {
 
 	pros::Task::delay(300);
 
-	while (!purePursuit.isDoneOrientation(0.1) && pros::millis() - startTime < 3000) {
+	while (!purePursuit.isDoneOrientation(0.1) && pros::millis() - startTime < 5000) {
 		pros::Task::delay(50);
 	}
 
@@ -138,7 +156,7 @@ void placeOnPlatform() {
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
 
-	liftButton.setAutonomousAuthority(1100);
+	liftButton.setAutonomousAuthority(1300);
 
 	pros::Task::delay(300);
 
@@ -176,7 +194,11 @@ void balanceRobot(double angle) {
 
 	drivetrain.skidSteerVelocity(200, 0);
 
-	pros::Task::delay(1500);
+	pros::Task::delay(300);
+
+	liftButton.setAutonomousAuthority(600);
+
+	pros::Task::delay(1200);
 
 	drivetrain.getLeftMotors().set_brake_mode(MOTOR_BRAKE_HOLD);
 	drivetrain.getRightMotors().set_brake_mode(MOTOR_BRAKE_HOLD);
@@ -214,13 +236,17 @@ void grabFromHook() {
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
 
+	pros::Task::delay(300);
+
+	double angle = goalAngle();
+
 	frontHookButton.setButtonStatus(POSITIVE);
 
 	pros::Task::delay(200);
 
 	drivetrain.skidSteerVelocity(-600, 0);
 
-	pros::Task::delay(250);
+	pros::Task::delay(150);
 
 	drivetrain.skidSteerVelocity(0, 0);
 
@@ -229,18 +255,29 @@ void grabFromHook() {
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
+	purePursuit.setSpeed(60);
+
+	turn(odometry.getPosition()->getTheta() + angle);
+
+	pros::Task::delay(200);
+
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
 
 	pros::Task::delay(200);
 
-	drivetrain.skidSteerVelocity(600, 0);
+	drivetrain.skidSteerVelocity(350, 0);
 
-	pros::Task::delay(500);
+	pros::Task::delay(550);
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
-	pros::Task::delay(200);
+	pros::Task::delay(50);
+
+	drivetrain.skidSteerVelocity(0, 0);
+
+	pros::Task::delay(400);
+	liftButton.setAutonomousAuthority(200);
 
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
@@ -259,6 +296,7 @@ int preAutonRun() {
 	liftButton.setAutonomous(true);
 	liftButton.setAutonomousPosition(true);
 	intakeButton.setAutonomous(true);
+	brakesButton.setAutonomous(true);
 
 	drivetrain.getLeftMotors().set_brake_mode(MOTOR_BRAKE_HOLD);
 	drivetrain.getRightMotors().set_brake_mode(MOTOR_BRAKE_HOLD);
@@ -266,13 +304,32 @@ int preAutonRun() {
 	return 0;
 }
 
+void deployBackGrabber() {
+	backGrabberManual = true;
+
+	backGrabberButton.setButtonStatus(POSITIVE);
+
+	pros::Task::delay(100);
+
+	backGrabberButton.setButtonStatus(NEUTRAL);
+
+	backGrabberManual = false;
+}
+
 void leftSteal() {
-	purePursuit.setSpeed(70);
+	purePursuit.setSpeed(75);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
+
+	purePursuit.setLookahead(30);
+	purePursuit.setMaxAcceleration(oldAccel * 2);
+
+	purePursuit.setUseVoltage(true);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
@@ -284,14 +341,35 @@ void leftSteal() {
 
 	purePursuit.setCurrentPathIndex(leftNeutralStealToLeftHomeZoneIndex);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+	purePursuit.setSpeed(65);
+
+	pros::Task::delay(200);
+
+	double lastY = odometry.getPosition()->getY();
+
+	while (odometry.getPosition()->getY() > 30) {
+		double y = odometry.getPosition()->getY();
+
+		if (y - lastY > 0.3) {
+			brakesButton.setButtonStatus(POSITIVE);
+		}
+
+		lastY = y;
+
+		pros::Task::delay(60);
 	}
 
+	purePursuit.setUseVoltage(false);
+
 	purePursuit.setSpeed(45);
 
 	waitForDone();
 
+	purePursuit.setLookahead(10);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
+
 	grabFromHook();
 }
 
@@ -299,9 +377,16 @@ void midSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
+
+	purePursuit.setLookahead(20);
+	purePursuit.setMaxAcceleration(oldAccel * 1.5);
+
+	purePursuit.setUseVoltage(true);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
@@ -311,16 +396,49 @@ void midSteal() {
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+	
 	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+	purePursuit.setUseVoltage(false);
+
+	purePursuit.setSpeed(50);
+
+	pros::Task::delay(200);
+
+	double lastY = odometry.getPosition()->getY();
+
+	while (odometry.getPosition()->getY() > 50) {
+		double y = odometry.getPosition()->getY();
+
+		if (y - lastY > 0.3) {
+			brakesButton.setButtonStatus(POSITIVE);
+		}
+
+		if (odometry.getPosition()->getY() < 50){
+			purePursuit.setSpeed(40);
+		}
+
+		lastY = y;
+
+		pros::Task::delay(60);
 	}
 
-	purePursuit.setSpeed(45);
+	purePursuit.setUseVoltage(false);
+
+	for (int i = 5; i > 2; i--) {
+		purePursuit.setSpeed(i * 10);
+		pros::Task::delay(300);
+	}
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
+
 	grabFromHook();
 }
 
@@ -328,28 +446,58 @@ void rightSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
+
+	purePursuit.setLookahead(30);
+	purePursuit.setMaxAcceleration(oldAccel * 2);
+
+	purePursuit.setUseVoltage(true);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
 	purePursuit.setCurrentPathIndex(rightHomeZoneToRightNeutralIndex);
 
-	waitForDone();
+	waitForDone(1);
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
+	waitForDone();
+
 	purePursuit.setCurrentPathIndex(rightNeutralToRightHomeZoneIndex);
 
+	purePursuit.setSpeed(65);
+
+	pros::Task::delay(200);
+
+	double lastY = odometry.getPosition()->getY();
+
 	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+		double y = odometry.getPosition()->getY();
+
+		if (y - lastY > 0.05) {
+			brakesButton.setButtonStatus(POSITIVE);
+		}
+
+		lastY = y;
+
+		pros::Task::delay(60);
 	}
 
+	purePursuit.setUseVoltage(false);
+
 	purePursuit.setSpeed(45);
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
+
 	grabFromHook();
 }
 
@@ -357,28 +505,58 @@ void rightRightSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
+
+	purePursuit.setLookahead(20);
+	purePursuit.setMaxAcceleration(oldAccel * 2);
+
+	purePursuit.setUseVoltage(true);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
 	purePursuit.setCurrentPathIndex(farRightHomeZoneToRightNeutralIndex);
 
-	waitForDone();
+	waitForDone(1);
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
+	waitForDone();
+
 	purePursuit.setCurrentPathIndex(rightNeutralToFarRightHomeZoneIndex);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+	purePursuit.setSpeed(65);
+
+	pros::Task::delay(200);
+
+	double lastY = odometry.getPosition()->getY();
+
+	while (odometry.getPosition()->getY() > 30) {
+		double y = odometry.getPosition()->getY();
+
+		if (y - lastY > 0.3) {
+			brakesButton.setButtonStatus(POSITIVE);
+		}
+
+		lastY = y;
+
+		pros::Task::delay(60);
 	}
 
+	purePursuit.setUseVoltage(false);
+
 	purePursuit.setSpeed(45);
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
+
 	grabFromHook();
 }
 
@@ -386,28 +564,60 @@ void rightMidSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
+
+	purePursuit.setLookahead(20);
+	purePursuit.setMaxAcceleration(oldAccel * 2);
+
+	purePursuit.setUseVoltage(true);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
 	purePursuit.setCurrentPathIndex(rightHomeZoneToMidNeutralIndex);
 
-	waitForDone();
+	waitForDone(1);
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
+	waitForDone();
+
+	purePursuit.setLookahead(oldLookahead);
+
 	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+	purePursuit.setSpeed(65);
+
+	pros::Task::delay(200);
+
+	double lastY = odometry.getPosition()->getY();
+
+	while (odometry.getPosition()->getY() > 30) {
+		double y = odometry.getPosition()->getY();
+
+		if (y - lastY > 0.3) {
+			brakesButton.setButtonStatus(POSITIVE);
+		}
+
+		lastY = y;
+
+		pros::Task::delay(60);
 	}
 
+	purePursuit.setUseVoltage(false);
+
 	purePursuit.setSpeed(45);
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
+
 	grabFromHook();
 }
 
@@ -416,101 +626,106 @@ void leftAWP() {
 
 	pros::Task::delay(100);
 
+	purePursuit.setSpeed(20);
+
 	purePursuit.setCurrentPathIndex(leftAllianceToRightHomeZoneIndex);
 
 	pros::Task::delay(200);
 
 	intakeButton.setButtonStatus(POSITIVE);
 
-	waitForDone(40);
+	waitForDone(20);
+
+	purePursuit.setSpeed(0);
+
+	pros::Task::delay(500);
+
+	purePursuit.setFollowing(false);
 }
 
 void rightAWP() {
-	turn(-M_PI_2 * 0.85);
-
 	purePursuit.setCurrentPathIndex(enterRightHomeZoneToRightAllianceIndex);
 
-	waitForDone(20);
+	waitForDone(20, 2000);
 
 	purePursuit.setSpeed(40);
 
-	waitForDone(8);
+	liftButton.setAutonomousAuthority(600);
 
-	backGrabberChange(true);
+	waitForDone(0.1, 2000);
 
-	waitForDone(0.1, 2500);
+	backGrabberChange(true);
 
 	pros::Task::delay(100);
 
+	purePursuit.setSpeed(60);
+
+	turn(-M_PI_4*0.5);
+
+	purePursuit.setSpeed(25);
+
 	purePursuit.setCurrentPathIndex(rightAllianceToRightRingsIndex);
 
 	liftButton.setAutonomousAuthority(2000);
 
+	pros::Task::delay(300);
+
 	intakeButton.setButtonStatus(POSITIVE);
 
 	waitForDone(0.1, 3000);
 
+	purePursuit.setSpeed(60);
+
 	purePursuit.setCurrentPathIndex(rightRingsToRightHomeZoneIndex);
 
+	waitForDone(5);
+
+	backGrabberChange(false);
+
 	waitForDone();
 }
 
-void rightAWPToLeftAWP() {
-	purePursuit.setSpeed(40);
+void rightToLeftAWP() {
 
-	purePursuit.setCurrentPathIndex(rightHomeZoneToRightAllianceIndex);
+	purePursuit.setSpeed(65);
 
-	waitForDone(8);
+	turn(-M_PI_2*0.6);
 
-	backGrabberChange(true);
+	purePursuit.setSpeed(50);
 
-	waitForDone(0.1, 3000);
+	purePursuit.setCurrentPathIndex(rightHomeZoneToLeftHomeZoneIndex);
 
-	intakeButton.setButtonStatus(POSITIVE);
+	waitForDone();
 
-	purePursuit.setCurrentPathIndex(rightAllianceToLeftAllianceIndex);
+	turn(-M_PI_4*0.7);
 
-	waitForDone(50);
+	purePursuit.setCurrentPathIndex(rightHomeZoneToLeftAllianceIndex);
 
-	intakeButton.setButtonStatus(NEUTRAL);
+	waitForDone(8);
 
-	turn(-M_PI_4);
+	purePursuit.setSpeed(20);
 
 	waitForDone();
 
 	backGrabberChange(true);
 
-	pros::Task::delay(200);
-
-	intakeButton.setButtonStatus(POSITIVE);
-
-	purePursuit.setSpeed(40);
-
-	for (int i = 0; i < 5; i++) {
-		purePursuit.setCurrentPathIndex(leftAllianceToPreloadsIndex);
-
-		waitForDone();
+	pros::Task::delay(800);
 
-		purePursuit.setCurrentPathIndex(preloadsToLeftAllianceIndex);
-
-		waitForDone();
-	}
+	liftButton.setAutonomousAuthority(600);
 
-	intakeButton.setButtonStatus(NEUTRAL);
-}
+	pros::Task::delay(100);
 
-void rightToLeftAWP() {
-	turn(-M_PI_2);
+	purePursuit.setSpeed(20);
 
-	purePursuit.setCurrentPathIndex(rightHomeZoneToLeftAllianceIndex);
+	purePursuit.setCurrentPathIndex(leftAllianceToRightHomeZoneIndex);
 
-	waitForDone();
+	pros::Task::delay(200);
 
-	backGrabberChange(true);
+	intakeButton.setButtonStatus(POSITIVE);
 
-	pros::Task::delay(200);
+	pros::Task::delay(500);
 
-	leftAWP();
+	backGrabberChange(false);
 }
 
 /*
@@ -519,34 +734,21 @@ void rightToLeftAWP() {
 
 /**
  * @brief Steal the left side goal and get the left AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int leftStealLeftAWP() {
-	odometry.reset(new Position(27, 18, 0.17));
-
-	leftSteal();
-
-	purePursuit.setCurrentPathIndex(leftHomeZoneToLeftAllianceIndex);
+	odometry.reset(new Position(27, 18, toRadians(10)));
 
-	waitForDone();
+	pros::Task deployBack(deployBackGrabber);
 
-	backGrabberChange(true);
+	leftSteal();
 
-	leftAWP();
+	purePursuit.setSpeed(60);
 
-	return 0;
-}
+	turn(-M_PI_4*0.5);
 
-/**
- * @brief Steal the left side goal and get the full AWP
- * 
- * @return int 
- */
-int leftStealFullAWP() {
-	odometry.reset(new Position(27, 18, 0.17));
-
-	leftSteal();
+	purePursuit.setSpeed(30);
 
 	purePursuit.setCurrentPathIndex(leftHomeZoneToLeftAllianceIndex);
 
@@ -556,9 +758,9 @@ int leftStealFullAWP() {
 
 	leftAWP();
 
-	waitForDone();
+	pros::Task::delay(1000);
 
-	rightAWP();
+	backGrabberChange(false);
 
 	return 0;
 }
@@ -569,106 +771,179 @@ int leftStealFullAWP() {
 
 /**
  * @brief Steal the right side goal and get the right AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int rightStealRightAWP() {
-	odometry.reset(new Position(105.7, 19.5, 0.0));
+	odometry.reset(new Position(107, 19.5, 0.0));
+
+	pros::Task deployBack(deployBackGrabber);
 
 	rightSteal();
+	
+	purePursuit.setSpeed(40);
+	turn(-M_PI_2 * 1.07);
 
 	rightAWP();
 
+	backGrabberChange(false);
+
+	pros::Task::delay(200);
+
 	return 0;
 }
 
 /**
- * @brief Steal the right side goal and get the full AWp
- * 
- * @return int 
+ * @brief Steal the right side goal starting from the right right side and get the right side AWP
+ *
+ * @return int
  */
-int rightStealFullAWP() {
-	odometry.reset(new Position(105.7, 19.5, 0.0));
+int rightRightStealToRightAWP() {
+	odometry.reset(new Position(121, 18, toRadians(-15)));
 
-	rightSteal();
+	pros::Task deployBack(deployBackGrabber);
 
-	rightAWPToLeftAWP();
+	rightRightSteal();
+
+	turn(-M_PI_2 * 1.4);
+
+	rightAWP();
+
+	backGrabberChange(false);
+
+	pros::Task::delay(200);
 
 	return 0;
 }
 
 /**
- * @brief Steal the right side goal starting from the right right side and get the right side AWP
- * 
- * @return int 
+ * @brief Steal the mid goal from the right side and get the Left AWP
+ *
+ * @return int
  */
-int rightRightStealToRightAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+int midStealToLeftAWP() {
+	odometry.reset(new Position(99.5, 18, toRadians(-30)));
 
-	rightRightSteal();
+	pros::Task::delay(200);
 
-	rightAWP();
+	pros::Task deployBack(deployBackGrabber);
+
+	midSteal();
+
+	rightToLeftAWP();
 
 	return 0;
 }
 
 /**
- * @brief Steal the right side goal starting from the right right side and get the right side AWP
- * 
- * @return int 
+ * @brief Steal the mid goal from the right side and get the right AWP
+ *
+ * @return int
  */
-int rightRightStealToFullAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+int midStealToRightAWP() {
+	odometry.reset(new Position(99.5, 18, toRadians(-30)));
 
-	rightRightSteal();
+	pros::Task deployBack(deployBackGrabber);
+
+	midSteal();
+	
+	turn(-M_PI_2 * 1.15);
+
+	rightAWP();
+
+	backGrabberChange(false);
+
+	return 0;
+}
+
+int rightDoubleSteal() {
+		purePursuit.setSpeed(70);
+
+	frontGrabberButton.setButtonStatus(NEUTRAL);
+	frontHookButton.setButtonStatus(POSITIVE);
+
+	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
+
+	purePursuit.setLookahead(20);
+	purePursuit.setMaxAcceleration(oldAccel * 1.5);
+
+	purePursuit.setUseVoltage(true);
+
+	purePursuit.setEnabled(true);
+	purePursuit.setFollowing(true);
+
+	purePursuit.setCurrentPathIndex(rightHomeZoneToMidNeutralIndex);
+
+	waitForDone();
+
+	frontHookButton.setButtonStatus(NEUTRAL);
+
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+	
+	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
+
+	purePursuit.setUseVoltage(false);
+
+	purePursuit.setSpeed(50);
+
+	pros::Task::delay(200);
+
+	double lastY = odometry.getPosition()->getY();
+
+	while (odometry.getPosition()->getY() > 50) {
+		double y = odometry.getPosition()->getY();
+
+		if (y - lastY > 0.3) {
+			brakesButton.setButtonStatus(POSITIVE);
+		}
+
+		if (odometry.getPosition()->getY() < 50){
+			purePursuit.setSpeed(40);
+		}
+
+		lastY = y;
+
+		pros::Task::delay(60);
+	}
+
+	purePursuit.setUseVoltage(false);
+
+	for (int i = 5; i > 2; i--) {
+		purePursuit.setSpeed(i * 10);
+		pros::Task::delay(300);
+	}
+
+	waitForDone(5);
+
+	frontHookButton.setButtonStatus(POSITIVE);
+
+	waitForDone();
+
+	frontHookButton.setButtonStatus(NEUTRAL);
 
-	rightAWPToLeftAWP();
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
 
-	return 0;
-}
+	brakesButton.setButtonStatus(NEUTRAL);
 
-/**
- * @brief Steal the mid goal from the right side and get the Left AWP
- * 
- * @return int 
- */
-int midStealToLeftAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	turn(0);
 
-	midSteal();
+	purePursuit.setSpeed(60);
 
-	rightToLeftAWP();
-	
-	return 0;
-}
+	purePursuit.setCurrentPathIndex(rightHomeZoneToRightNeutralClawIndex);
 
-/**
- * @brief Steal the mid goal from the right side and get the Left AWP
- * 
- * @return int 
- */
-int midStealToRightAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	waitForDone();
 
-	midSteal();
+	frontGrabberButton.setButtonStatus(POSITIVE);
 
-	rightAWP();
-	
-	return 0;
-}
+	pros::Task::delay(100);
 
-/**
- * @brief Steal the mid goal from the right side and get the full AWP
- * 
- * @return int 
- */
-int midStealToFullAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	purePursuit.setCurrentPathIndex(rightHomeZoneToRightNeutralClawIndex);
 
-	midSteal();
+	waitForDone();
 
-	rightAWPToLeftAWP();
-	
 	return 0;
 }
 
@@ -732,284 +1007,182 @@ int skills() {
 
 	liftButton.setAutonomousAuthority(2000);
 
+	while(odometry.getPosition()->getX() < 55) {
+		pros::Task::delay(50);
+	}
+
+	purePursuit.setSpeed(30);
+
 	waitForDone(0.5, 3000);
 
 	purePursuit.setSpeed(60);
 
-	turn(-M_PI_4 * 0.66, 0.3);
-
 	turn(0, 0.1);
 
 	purePursuit.setFollowing(false);
 
 	placeOnPlatform();
 
-	turn(-M_PI_2);
-
-	purePursuit.setCurrentPathIndex(farLeftHomeZoneToFarRightGoalDropOffIndex);
-
-	waitForDone(2);
-
-	intakeButton.setButtonStatus(NEUTRAL);
-
-	backGrabberChange(false);
-
-	pros::Task::delay(600);
+	// Far platform 
+	purePursuit.setCurrentPathIndex(farPlatformToEnterFarHomeZoneIndex);
 
 	waitForDone();
 
-	purePursuit.setSpeed(60);
-
-	turn(M_PI_2 + M_PI_4);
-
-	purePursuit.setCurrentPathIndex(farRightDropOffToFarLeftAllianceGoalIndex);
-
-	purePursuit.setSpeed(60);
-
-	waitForDone(40);
-
-	liftButton.setAutonomousAuthority(600);
-
-	purePursuit.setEnabled(false);
-	purePursuit.setFollowing(false);
-
-	pros::Task::delay(500);
-
-	purePursuit.setEnabled(true);
-	purePursuit.setFollowing(true);
+	liftButton.setAutonomousAuthority(0);
 
-	turn(M_PI_2);
+	purePursuit.setSpeed(30);
 
-	purePursuit.setSpeed(20);
+	turn(M_PI*0.8);
 
-	waitForDone(0.5, 2000);
+	purePursuit.setCurrentPathIndex(enterFarHomeZoneToMidGoalIndex);
 
-	backGrabberChange(true);
+	intakeButton.setButtonStatus(POSITIVE);
 
-	pros::Task::delay(100);
+	// Slow down on the way to the tall goal
 
 	purePursuit.setSpeed(30);
 
-	turn(M_PI_2 + M_PI_4);
-
-	purePursuit.setCurrentPathIndex(farLeftAllianceToMidGoalIndex);
-
-	intakeButton.setButtonStatus(POSITIVE);
+	waitForDone(30);
 
 	purePursuit.setSpeed(20);
 
-	waitForDone(15);
-
-	liftButton.setAutonomousAuthority(0);
-
 	waitForDone();
 
+	// Grab the middle goal
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
 	pros::Task::delay(300);
 
-	purePursuit.setSpeed(60);
-
-	turn(0);
-
-	purePursuit.setCurrentPathIndex(midGoalToFarPlatformIndex);
-
-	liftButton.setAutonomousAuthority(2000);
-
-	purePursuit.setSpeed(20);
-
-	placeOnPlatform();
-
-	turn(M_PI_2);
-
-	purePursuit.setCurrentPathIndex(farPlatformToFarLeftAllianceDropOffIndex);
-
-	liftButton.setAutonomousAuthority(0);
-
-	waitForDone();
-
-	turn(M_PI_2);
-
-	intakeButton.setButtonStatus(NEUTRAL);
-
-	pros::Task::delay(200);
-
-	backGrabberChange(false);
+	liftButton.setAutonomousAuthority(600);
 
 	pros::Task::delay(500);
 
-	purePursuit.setCurrentPathIndex(farLeftAllianceDropOffToFarRightDropOffIndex);
+	purePursuit.setSpeed(60);
 
-	waitForDone();
+	turn(0);
 
-	frontGrabberButton.setButtonStatus(POSITIVE);
+	turn(0);
 
-	pros::Task::delay(200);
+	// Place the middle goal on the platform
+	purePursuit.setCurrentPathIndex(midGoalToFarPlatformIndex);
 
 	liftButton.setAutonomousAuthority(2000);
 
-	pros::Task::delay(1000);
-
-	turn(0);
-
-	pros::Task::delay(200);
-
-	purePursuit.setCurrentPathIndex(farRightDropOffToPlatformIndex);
+	purePursuit.setSpeed(40);
 
 	placeOnPlatform();
 
-	purePursuit.setCurrentPathIndex(platformToEnterFarRightHomeZoneIndex);
-
-	waitForDone();
-
-	turn(-M_PI_2);
-
-	purePursuit.setCurrentPathIndex(enterRightHomeZoneToFarRightAllianceIndex);
+	// Move to drop off the alliance goal
+	purePursuit.setCurrentPathIndex(farPlatformToRightAllianceDropOffIndex);
 
 	waitForDone();
 
-	backGrabberChange(true);
-
-	pros::Task::delay(200);
-
-	purePursuit.setCurrentPathIndex(farRightAllianceToRightHomeZoneIndex);
-
-	pros::Task::delay(200);
-
-	intakeButton.setButtonStatus(POSITIVE);
-
-	while (odometry.getPosition()->getY() > 85) {
-		pros::Task::delay(50);
-	}
+	liftButton.setAutonomousAuthority(0);
 
-	purePursuit.setSpeed(50);
+	// Flip goal to front
+	{
+		backGrabberChange(false);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
-	}
+		pros::Task::delay(400);
 
-	purePursuit.setSpeed(10);
+		purePursuit.setEnabled(false);
+		purePursuit.setFollowing(false);
 
-	while (odometry.getPosition()->getY() > 35) {
-		pros::Task::delay(50);
-	}
+		drivetrain.skidSteerVelocity(300, 0);
 
-	backGrabberChange(false);
+		pros::Task::delay(150);
 
-	intakeButton.setButtonStatus(NEUTRAL);
+		purePursuit.setEnabled(true);
+		purePursuit.setFollowing(true);
 
-	pros::Task::delay(500);
+		turn(odometry.getPosition()->getTheta() + M_PI);
 
-	purePursuit.setSpeed(60);
+		purePursuit.setEnabled(false);
+		purePursuit.setFollowing(false);
 
-	waitForDone();
+		pros::Task::delay(2000);
 
-	turn(-M_PI_2 - M_PI_4);
+		drivetrain.skidSteerVelocity(300, 0);
 
-	purePursuit.setCurrentPathIndex(rightHomeZoneToRightAllianceIndex);
+		pros::Task::delay(400);
 
-	waitForDone();
+		purePursuit.setEnabled(true);
+		purePursuit.setFollowing(true);
 
-	backGrabberChange(true);
+		frontGrabberButton.setButtonStatus(POSITIVE);
+	}
 
-	pros::Task::delay(100);
+	liftButton.setAutonomousAuthority(2000);
 
 	turn(0);
 
-	purePursuit.setCurrentPathIndex(rightAllianceToRightNeutralIndex);
+	// Place near alliance goal on platform
+	purePursuit.setCurrentPathIndex(rightAllianceToFarPlatformIndex);
 
 	waitForDone();
 
-	frontGrabberButton.setButtonStatus(POSITIVE);
-
-	purePursuit.setCurrentPathIndex(rightNeutralToPlatformIndex);
-
-	pros::Task::delay(500);
-
-	liftButton.setAutonomousAuthority(2000);
+	turn(0);
 
 	placeOnPlatform();
 
-	purePursuit.setCurrentPathIndex(rightPlatformToFarRightGoalDropOff2Index);
-
-	liftButton.setAutonomousAuthority(0);
+	// Move to the far right alliance goal
+	purePursuit.setCurrentPathIndex(platformToEnterFarRightHomeZoneIndex);
 
 	waitForDone();
 
-	turn(-M_PI_2);
-
-	backGrabberChange(false);
-	intakeButton.setButtonStatus(NEUTRAL);
-
-	purePursuit.setCurrentPathIndex(farRightGoalDropOff2ToFarLeftAllianceDropOffIndex);
-
-	waitForDone(20);
-
 	turn(M_PI_2);
 
-	purePursuit.setCurrentPathIndex(farLeftAllianceDropOffToFarRightGoalDropOff2Index);
-
-	waitForDone();
-
-	frontGrabberButton.setButtonStatus(POSITIVE);
-
-	pros::Task::delay(200);
+	purePursuit.setSpeed(40);
 
-	purePursuit.setCurrentPathIndex(farRightGoalDropOff2ToFarLeftAllianceDropOffIndex);
+	purePursuit.setCurrentPathIndex(farPlatformToFarLeftAllianceGoalIndex);
 
-	pros::Task::delay(500);
+	waitForDone(20, 4000);
 
-	liftButton.setAutonomousAuthority(2000);
+	purePursuit.setSpeed(20);
 
-	waitForDone();
+	waitForDone(0.5, 4000);
 
 	backGrabberChange(true);
 
-	pros::Task::delay(100);
-
-	intakeButton.setButtonStatus(POSITIVE);
-
-	turn(0);
-
-	purePursuit.setCurrentPathIndex(farLeftAllianceDropOffToPlatformIndex);
-
-	placeOnPlatform();
+	pros::Task::delay(500);
 
-	turn(M_PI_2);
+	purePursuit.setSpeed(40);
 
-	purePursuit.setCurrentPathIndex(enterFarLeftHomeZoneToNearRightPlatformIndex);
+	purePursuit.setCurrentPathIndex(farLeftAllianceGoalToRightNeutralGoalIndex);
 
 	liftButton.setAutonomousAuthority(600);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
-	}
+	waitForDone(40);
 
 	purePursuit.setSpeed(20);
 
 	liftButton.setAutonomousAuthority(0);
 
-	while (odometry.getPosition()->getY() > 35) {
-		pros::Task::delay(50);
-	}
+	waitForDone();
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
-	pros::Task::delay(100);
+	pros::Task::delay(150);
+
+	liftButton.setAutonomousAuthority(2000);
 
-	liftButton.setAutonomousAuthority(1600);
+	turn(-M_PI_2-M_PI_4);
 
 	purePursuit.setSpeed(40);
 
+	purePursuit.setCurrentPathIndex(rightNeutralGoalToNearRIghtPlatformIndex);
+
 	waitForDone();
 
-	turn(-M_PI_2);
+	pros::Task::delay(500);
+
+	turn(M_PI_2);
 
 	liftButton.setAutonomousAuthority(100);
 
-	pros::Task::delay(1000);
+	pros::Task::delay(2000);
 
-	balanceRobot(-M_PI_2);
+	balanceRobot(M_PI_2);
 
 	pros::Task::delay(500);
 
@@ -1128,6 +1301,7 @@ int postAuton() {
 	backGrabberButton.setAutonomous(false);
 	liftButton.setAutonomous(false);
 	intakeButton.setAutonomous(false);
+	brakesButton.setAutonomous(false);
 
 	balance.setEnabled(false);
 
@@ -1148,26 +1322,112 @@ void renderThread() {
 }
 
 void updateDrivetrain() {
-	lv_obj_t* infoLabel = lv_label_create(lv_scr_act(), NULL);
-	lv_label_set_text(infoLabel, "drivetrain");
 
 	while (1) {
 		uint32_t startTime = pros::millis();
 		odometry.update();
 
-		if (purePursuit.isEnabled()) {
-			purePursuit.update();
-		}
-		else if (balance.isEnabled()) {
-			balance.update();
-		}
-
-		lv_label_set_text(infoLabel, (odometry.getPosition()->to_string() + "\nL: " + std::to_string(leftOdomWheel.getPosition()) + ", R: " + std::to_string(rightOdomWheel.getPosition())).c_str());
+		purePursuit.update();
+		balance.update();
 
+		// Has to be in increments of 10 because of the VEX sensor update time
 		pros::Task::delay_until(&startTime, 20);
 	}
 }
 
+void lvChartAddValue(lv_chart_series_t* chartSeries, int size, int value) {
+	for (int i = 0; i < size - 1; i++) {
+		chartSeries->points[i] = chartSeries->points[i + 1];
+	}
+
+	chartSeries->points[size - 1] = value;
+}
+
+void chartInit(lv_chart_series_t* chartSeries, int size) {
+	for (int i = 0; i < size; i++) {
+		chartSeries->points[i] = 0;
+	}
+}
+
+void updateDisplay() {
+
+	// Odom
+	lv_obj_t* odomTab = lv_tabview_add_tab(tabview, "Odom");
+	lv_obj_t* odomLabel = lv_label_create(odomTab, NULL);
+
+	// Odom
+	lv_obj_t* purePursuitTab = lv_tabview_add_tab(tabview, "Pure pursuit");
+	lv_obj_t* purePursuitLabel = lv_label_create(purePursuitTab, NULL);
+
+	// Balance
+	lv_obj_t* balanceTab = lv_tabview_add_tab(tabview, "Balance");
+	lv_obj_t* balanceLabel = lv_label_create(balanceTab, NULL);
+
+	// Drivetrain
+	lv_obj_t* drivetrainTab = lv_tabview_add_tab(tabview, "Drivetrain");
+	lv_obj_t* drivetrainTable = lv_table_create(drivetrainTab, NULL);
+
+	lv_table_set_row_cnt(drivetrainTable, 3);
+	lv_table_set_col_cnt(drivetrainTable, 2);
+
+	lv_table_set_col_width(drivetrainTable, 0, 200);
+	lv_table_set_col_width(drivetrainTable, 1, 200);
+
+	// Intake
+	lv_obj_t* intakeTab = lv_tabview_add_tab(tabview, "Intake");
+	lv_obj_t* intakeChart = lv_chart_create(intakeTab, NULL);
+	lv_obj_set_size(intakeChart, 200, 140);
+	lv_chart_set_type(intakeChart, LV_CHART_TYPE_LINE);
+
+	lv_chart_series_t* intakeInputSpeed = lv_chart_add_series(intakeChart, LV_COLOR_BLUE);
+	lv_chart_series_t* intakeOutputSpeed = lv_chart_add_series(intakeChart, LV_COLOR_RED);
+
+	// Vision
+	lv_obj_t* visionTab = lv_tabview_add_tab(tabview, "Vision");
+	lv_obj_t* visionLabel = lv_label_create(visionTab, NULL);
+	lv_obj_t* visionResultLabel = lv_label_create(visionTab, NULL);
+
+	//chartInit(intakeInputSpeed, 50);
+	//chartInit(intakeOutputSpeed, 50);
+//
+	//pros::Task::delay(100);
+
+	while (1) {
+		// Odometry
+		lv_label_set_text(odomLabel, (odometry.getPosition()->to_string()
+			+ "\nL: " + std::to_string(leftOdomWheel.getPosition()) +
+			", R: " + std::to_string(rightOdomWheel.getPosition())).c_str());
+
+		// Balance
+		lv_label_set_text(balanceLabel, ("Angle: " + std::to_string(balance.getLinearController()->getLastInput()) + "\n").c_str());
+
+		// Drivetrain
+		lv_table_set_cell_value(drivetrainTable, 0, 0, (std::to_string(frontLeftMotor.get_temperature()) + " C").c_str());
+		lv_table_set_cell_value(drivetrainTable, 1, 0, (std::to_string(midLeftMotor.get_temperature()) + " C").c_str());
+		lv_table_set_cell_value(drivetrainTable, 2, 0, (std::to_string(backLeftMotor.get_temperature()) + " C").c_str());
+		lv_table_set_cell_value(drivetrainTable, 0, 1, (std::to_string(frontRightMotor.get_temperature()) + " C").c_str());
+		lv_table_set_cell_value(drivetrainTable, 1, 1, (std::to_string(midRightMotor.get_temperature()) + " C").c_str());
+		lv_table_set_cell_value(drivetrainTable, 2, 1, (std::to_string(backRightMotor.get_temperature()) + " C").c_str());
+
+		// Pure pursuit
+		lv_label_set_text(purePursuitLabel, ("Distance Remaining on path: " + std::to_string(purePursuit.getPointData().distanceFromEnd)).c_str());
+
+		// Intake
+		//addValue(intakeInputSpeed, 50, intake.get_target_velocity());
+		//addValue(intakeOutputSpeed, 50, intake.get_actual_velocity());
+
+		lv_label_set_text(visionLabel, ("X coordinate:" + std::to_string(clawVision.get_by_sig(0, 1).x_middle_coord) + "\nAngle: " + std::to_string(toDegrees(goalAngle())) + "\nError: " + std::to_string(toDegrees(goalAngle() + odometry.getPosition()->getTheta()))).c_str());
+
+		//lv_chart_refresh(intakeChart);
+
+		pros::Task::delay(100);
+	}
+}
+
+void initDisplay() {
+	pros::Task updateDisplayTask = pros::Task(updateDisplay, "Update dispay");
+}
+
 /**
  * Initialize all sensors
  */
@@ -1200,13 +1460,13 @@ void updateMotors() {
 			}
 		}
 
-		if (backButtonStatus) {
+		if (backButtonStatus && !backGrabberManual) {
 			backGrabberButton2.setButtonStatus(POSITIVE);
-			if (pros::millis() - lastChange > 100) {
+			if (pros::millis() - lastChange > 150) {
 				backGrabberButton.setButtonStatus(POSITIVE);
 			}
 		}
-		else {
+		else if (!backGrabberManual) {
 			backGrabberButton.setButtonStatus(NEUTRAL);
 			if (pros::millis() - lastChange > 150) {
 				backGrabberButton2.setButtonStatus(NEUTRAL);
@@ -1220,6 +1480,8 @@ void updateMotors() {
 
 		intakeButton.update();
 
+		brakesButton.update();
+
 		pros::Task::delay(20);
 	}
 }
@@ -1248,6 +1510,9 @@ void initMotors() {
 	backGrabberButton2.setSolenoid(&backGrabber);
 	backGrabberButton2.setSingleToggle(true);
 
+	brakesButton.setSolenoid(&brakes);
+	brakesButton.setSingleToggle(true);
+
 	intakeButton.setSingleToggle(true);
 	intakeButton.setDejam(true);
 	intakeButton.setDejamAuthority(-200);
@@ -1267,8 +1532,8 @@ void initDrivetrain() {
 	// 1.072124756
 	// Left 99.57
 	// Right 100.57
-	double turningFactor = (((101.22 / 101.50) - 1.0) / 2);
-	double tuningFactor = 1.010901883;
+	double turningFactor = (((100.35 / 100.0) - 1.0) / 2);
+	double tuningFactor = 0.998791278;
 	leftOdomWheel.setRadius(2.75 / 2);
 	leftOdomWheel.setTuningFactor(tuningFactor * (1 - turningFactor));
 	rightOdomWheel.setRadius(2.75 / 2);
@@ -1344,6 +1609,13 @@ void initLogger() {
 	Logger::getDefaultLogger()->debug<std::string>("LOGGER: Logger initialized");
 }
 
+void initVision() {
+	pros::vision_signature_s_t YELLOW_GOAL = pros::Vision::signature_from_utility(1, 1887, 3735, 2810, -4021, -3533, -3778, 3.000, 0);
+	clawVision.set_signature(0, &YELLOW_GOAL);
+	clawVision.set_auto_white_balance(0);
+	clawVision.set_exposure(20);
+}
+
 /**
  * Filter and apply the quadratic function.
  */
@@ -1368,6 +1640,7 @@ void initialize() {
 	printf("Initialize");
 
 	lv_init();
+	tabview = lv_tabview_create(lv_scr_act(), NULL);
 
 	printf("LVGL Init");
 
@@ -1378,6 +1651,8 @@ void initialize() {
 	initDrivetrain();
 	initController();
 	initLogger();
+	initDisplay();
+	initVision();
 	// initSelector();
 
 	printf("Init done\n");
@@ -1472,24 +1747,25 @@ void opcontrol() {
 			int leftY = filterAxis(master, ANALOG_LEFT_Y);
 			int rightX = filterAxis(master, ANALOG_RIGHT_X);
 
-			drivetrain.skidSteerVelocity(leftY, rightX);
+			if (brakesButton.getButtonStatus() == POSITIVE) {
+				drivetrain.skidSteerVelocity(leftY < 0 ? leftY : 0, rightX);
+			}
+			else {
+				drivetrain.skidSteerVelocity(leftY, rightX);
+			}
 		}
 		else if (!balance.isEnabled()) {
 			int leftY = filterAxis(master, ANALOG_LEFT_Y);
 			int rightY = filterAxis(master, ANALOG_RIGHT_Y);
 
-			drivetrain.tankSteerVelocity(leftY, rightY);
-		}
-
-		if (frontGrabberBumperSwitch.get_new_press()) {
-			frontGrabberButton.setButtonStatus(Pronounce::ButtonStatus::POSITIVE);
-		}
-
-		if (backGrabberBumperSwitch.get_new_press()) {
-			backGrabberButton.setButtonStatus(Pronounce::ButtonStatus::POSITIVE);
+			if (brakesButton.getButtonStatus() == POSITIVE) {
+				drivetrain.tankSteerVelocity(leftY < 0 ? leftY : 0, rightY < 0 ? rightY : 0);
+			}
+			else {
+				drivetrain.tankSteerVelocity(leftY, rightY);
+			}
 		}
 
-
 		if (master.get_digital_new_press(DIGITAL_RIGHT)) {
 			drivetrain.getLeftMotors().set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
 			drivetrain.getRightMotors().set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
@@ -1499,13 +1775,6 @@ void opcontrol() {
 			drivetrain.getRightMotors().set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
 		}
 
-		if (master.get_digital_new_press(DIGITAL_B)) {
-			balance.setEnabled(!balance.isEnabled());
-			balance.getOrientationController()->setTarget(imu.get_heading());
-			drivetrain.getLeftMotors().set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
-			drivetrain.getRightMotors().set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
-		}
-
 		// if (master.get_digital_new_press(DIGITAL_X)) {
 		// 	odometry.reset(new Position());
 		// }
@@ -1534,6 +1803,14 @@ void opcontrol() {
 			else if (master.get_digital_new_press(DIGITAL_Y) || partner.get_digital_new_press(DIGITAL_R1)) {
 				intakeButton.setButtonStatus(intakeButton.getButtonStatus() == NEGATIVE ? NEUTRAL : NEGATIVE);
 			}
+
+			if (partner.get_digital_new_press(DIGITAL_A)) {
+				purePursuit.setEnabled(true);
+				purePursuit.setFollowing(true);
+				turn(odometry.getPosition()->getTheta() + goalAngle());
+				purePursuit.setEnabled(false);
+				purePursuit.setFollowing(false);
+			}
 		}
 		else {
 			if (master.get_digital(DIGITAL_L1) && !(master.get_digital(DIGITAL_L1) && master.get_digital(DIGITAL_L2))) {
diff --git a/src/motionControl/purePursuit.cpp b/src/motionControl/purePursuit.cpp
index 2d3c9bcc..6c0bb7c8 100644
--- a/src/motionControl/purePursuit.cpp
+++ b/src/motionControl/purePursuit.cpp
@@ -50,11 +50,14 @@ namespace Pronounce {
 		double xDistance = robotRelativeLookaheadVector.getCartesian().getX();
 		double signedCurvature = (2 * xDistance) / pow(lookaheadVector.getMagnitude(), 2);
 
+		double distanceFromEnd = path.distanceFromEnd(currentPoint);
+
 		this->pointData.lookaheadPoint = lookaheadPoint;
 		this->pointData.lookaheadVector = lookaheadVector;
 		this->pointData.localLookaheadVector = robotRelativeLookaheadVector;
 		this->pointData.normalizedLookaheadVector = normalizedLookaheadVector;
 		this->pointData.curvature = signedCurvature;
+		this->pointData.distanceFromEnd = distanceFromEnd;
     }
 
     PurePursuit::~PurePursuit() {
diff --git a/src/motionControl/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp
index bfb9a179..10b3c36e 100644
--- a/src/motionControl/tankPurePursuit.cpp
+++ b/src/motionControl/tankPurePursuit.cpp
@@ -65,7 +65,7 @@ namespace Pronounce {
 		Path currentPath = this->getPath(this->getCurrentPathIndex());
 		Point currentPoint = Point(this->getOdometry()->getPosition()->getX(), this->getOdometry()->getPosition()->getY());
 
-		double maxSpeed = sqrt(2 * this->getMaxAcceleration() * this->getPath(this->getCurrentPathIndex()).distanceFromEnd(Point(this->getOdometry()->getPosition()->getX(), this->getOdometry()->getPosition()->getY())) + 2.0);
+		double maxSpeed = sqrt(2 * this->getMaxAcceleration() * pointData.distanceFromEnd + 2.0);
 		maxSpeed = maxSpeed > 5 ? maxSpeed : 5;
 
 		double updateTime = pros::millis() - lastUpdateTime;
@@ -74,7 +74,8 @@ namespace Pronounce {
 		double speed = 0;
 		if (maxSpeed > speed) {
 			speed = clamp(this->getSpeed() * side, this->drivetrain->getSpeed() - maxAccelerationFrame, this->drivetrain->getSpeed() + maxAccelerationFrame);
-		} else {
+		}
+		else {
 			speed = this->getSpeed() * side;
 		}
 
@@ -86,9 +87,13 @@ namespace Pronounce {
 		// printf(std::string("Path: " + currentPath.getName()).c_str());
 		// printf("\n\n");
 
-		double motorSpeed = clamp(clamp(speed, -maxSpeed, maxSpeed), -this->getSpeed(), this->getSpeed())  * this->getOutputMultiplier();
+		double motorSpeed = clamp(clamp(speed, -maxSpeed, maxSpeed), -this->getSpeed(), this->getSpeed()) * this->getOutputMultiplier();
 
-		drivetrain->driveCurvature(motorSpeed, pointData.curvature);
+		if (useVoltage) {
+			drivetrain->driveCurvatureVoltage(motorSpeed * (12000/600.0), pointData.curvature);
+		} else {
+			drivetrain->driveCurvature(motorSpeed, pointData.curvature);
+		}
 	}
 
 	void TankPurePursuit::stop() {
diff --git a/src/odometry/threeWheelOdom.cpp b/src/odometry/threeWheelOdom.cpp
index 12c33cb6..bcaca48c 100644
--- a/src/odometry/threeWheelOdom.cpp
+++ b/src/odometry/threeWheelOdom.cpp
@@ -48,24 +48,27 @@ namespace Pronounce {
 		double deltaAngle = (deltaLeft - deltaRight) / (leftOffset + rightOffset);
 		double averageOrientation = lastAngle + (deltaAngle / 2);
 
-		Vector localOffset;
+		Point localOffset;
 
-		if (abs(deltaAngle) > 0.01) {
+		if (deltaAngle != 0.0) {
 			double rotationAdjustment = 2 * sin(deltaAngle / 2);
-			localOffset = Vector(new Point(((deltaBack/deltaAngle) + backOffset) * rotationAdjustment, ((deltaRight/deltaAngle) + rightOffset) * rotationAdjustment));
+			localOffset = Point(((deltaBack/deltaAngle) + backOffset) * rotationAdjustment, ((deltaRight/deltaAngle) + rightOffset) * rotationAdjustment);
 		} else {
 			// Calculate the local offset then translate it to the global offset
-			localOffset = Vector(new Point(deltaBack, deltaRight));
+			localOffset = Point(deltaBack, deltaRight);
 		}
 
-		// Rotate vector
-		localOffset.rotate(-averageOrientation);
+		// Rotate local offset
+		double rotationCos = cos(averageOrientation);
+		double rotationSin = sin(averageOrientation);
+
+		localOffset = Point(localOffset.getX() * rotationCos + localOffset.getY() * rotationSin, - localOffset.getX() * rotationSin + localOffset.getY() * rotationCos);
 
 		// Add localOffset to the global offset
-		lastPosition->add(localOffset.getCartesian());
+		lastPosition->add(localOffset);
 		lastPosition->setTheta(fmod(angleDifference(currentAngle, 0) + M_PI * 2, M_PI * 2));
 
-		if (localOffset.getMagnitude() > maxMovement && maxMovement != 0) {
+		if (Vector(&localOffset).getMagnitude() > maxMovement && maxMovement != 0) {
 			return;
 		}