From d388a4afa9c768c964e1be96c48cd2803ec9daa9 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Tue, 19 Apr 2022 16:13:58 -0600
Subject: [PATCH 01/13] Left side steal + AWP working

---
 include/autoPaths.hpp                    |  16 +--
 include/feedbackControllers/bangBang.hpp |   4 +
 src/main.cpp                             | 153 +++++++++++++++++------
 3 files changed, 126 insertions(+), 47 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index fe5f0699..701408ad 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -273,15 +273,15 @@ namespace Pronounce {
 
 		QuadraticSplinePath rightHomeZoneToRightNeutral;
 
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(105.7, 19.5), Vector(0.0, 0)));
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(105.7, 46), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 46), Vector(0.0, 0)));
 
 		rightHomeZoneToRightNeutralIndex = purePursuit->addPath(rightHomeZoneToRightNeutral.getPath(0.01));
 
 		QuadraticSplinePath rightNeutralToRightHomeZone;
 
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(105.7, 46), Vector(0.0, 0)));
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(105.7, 17), Vector(0.0, 0)));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 46), Vector(0.0, 0)));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 17), Vector(0.0, 0)));
 
 		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.01));
 
@@ -326,7 +326,7 @@ namespace Pronounce {
 		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)));
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(35, 20), Vector(15.0, M_PI + M_PI_4)));
 
 		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.01));
 
@@ -334,7 +334,7 @@ namespace Pronounce {
 		
 		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)));
+		leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(98, 40.0), Vector(10.0, -M_PI_2)));
 
 		leftAllianceToRightHomeZoneIndex = purePursuit->addPath(leftAllianceToRightHomeZone.getPath(0.01));
 
@@ -367,7 +367,7 @@ namespace Pronounce {
 		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)));
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(122, 37), Vector(15, - M_PI_2 - 0.4)));
 
 		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.01));
 
@@ -383,7 +383,7 @@ namespace Pronounce {
 		QuadraticSplinePath rightRingsToRightHomeZone;
 
 		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(20.0, M_PI_2)));
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(135, 25), Vector(15, M_PI)));
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(120, 25), Vector(15, M_PI)));
 
 		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.01));
 
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/src/main.cpp b/src/main.cpp
index 87a5b214..ea3cdbe2 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -60,6 +60,9 @@ SolenoidButton backGrabberButton2(&master, DIGITAL_R1, DIGITAL_R1);
 // Autonomous Selector
 Pronounce::AutonSelector autonomousSelector;
 
+// LVGL
+lv_obj_t* tabview;
+
 #define DRIFT_MIN 7.0
 
 bool preDriverTasksDone = false;
@@ -68,6 +71,7 @@ bool disableIntake = true;
 int driverMode = 0;
 
 bool backButtonStatus = false;
+bool backGrabberManual = false;
 uint32_t lastChange = 0;
 
 // SECTION Auton
@@ -229,6 +233,8 @@ void grabFromHook() {
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
+	turn(odometry.getPosition()->getTheta() - 0.1);
+
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
 
@@ -266,6 +272,18 @@ 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);
 
@@ -273,6 +291,10 @@ void leftSteal() {
 	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+
+	purePursuit.setLookahead(25);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
@@ -292,6 +314,8 @@ void leftSteal() {
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+
 	grabFromHook();
 }
 
@@ -331,6 +355,10 @@ void rightSteal() {
 	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+
+	purePursuit.setLookahead(20);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
@@ -350,6 +378,8 @@ void rightSteal() {
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+
 	grabFromHook();
 }
 
@@ -416,13 +446,21 @@ 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() {
@@ -430,11 +468,11 @@ void rightAWP() {
 
 	purePursuit.setCurrentPathIndex(enterRightHomeZoneToRightAllianceIndex);
 
-	waitForDone(20);
+	waitForDone(20, 2500);
 
-	purePursuit.setSpeed(40);
+	purePursuit.setSpeed(25);
 
-	waitForDone(8);
+	waitForDone(8, 2500);
 
 	backGrabberChange(true);
 
@@ -519,12 +557,14 @@ 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));
 
+	pros::Task deployBack(deployBackGrabber);
+
 	leftSteal();
 
 	purePursuit.setCurrentPathIndex(leftHomeZoneToLeftAllianceIndex);
@@ -535,13 +575,17 @@ int leftStealLeftAWP() {
 
 	leftAWP();
 
+	pros::Task::delay(2000);
+
+	backGrabberChange(false);
+
 	return 0;
 }
 
 /**
  * @brief Steal the left side goal and get the full AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int leftStealFullAWP() {
 	odometry.reset(new Position(27, 18, 0.17));
@@ -569,23 +613,29 @@ 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();
 
 	rightAWP();
 
+	backGrabberChange(false);
+
+	pros::Task::delay(200);
+
 	return 0;
 }
 
 /**
  * @brief Steal the right side goal and get the full AWp
- * 
- * @return int 
+ *
+ * @return int
  */
 int rightStealFullAWP() {
 	odometry.reset(new Position(105.7, 19.5, 0.0));
@@ -594,17 +644,23 @@ int rightStealFullAWP() {
 
 	rightAWPToLeftAWP();
 
+	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 
+ *
+ * @return int
  */
 int rightRightStealToRightAWP() {
 	odometry.reset(new Position(115, 19.5, 0.0));
 
+	pros::Task deployBack(deployBackGrabber);
+
 	rightRightSteal();
 
 	rightAWP();
@@ -614,8 +670,8 @@ int rightRightStealToRightAWP() {
 
 /**
  * @brief Steal the right side goal starting from the right right side and get the right side AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int rightRightStealToFullAWP() {
 	odometry.reset(new Position(115, 19.5, 0.0));
@@ -629,8 +685,8 @@ int rightRightStealToFullAWP() {
 
 /**
  * @brief Steal the mid goal from the right side and get the Left AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int midStealToLeftAWP() {
 	odometry.reset(new Position(115, 19.5, 0.0));
@@ -638,14 +694,14 @@ int midStealToLeftAWP() {
 	midSteal();
 
 	rightToLeftAWP();
-	
+
 	return 0;
 }
 
 /**
  * @brief Steal the mid goal from the right side and get the Left AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int midStealToRightAWP() {
 	odometry.reset(new Position(115, 19.5, 0.0));
@@ -653,14 +709,14 @@ int midStealToRightAWP() {
 	midSteal();
 
 	rightAWP();
-	
+
 	return 0;
 }
 
 /**
  * @brief Steal the mid goal from the right side and get the full AWP
- * 
- * @return int 
+ *
+ * @return int
  */
 int midStealToFullAWP() {
 	odometry.reset(new Position(115, 19.5, 0.0));
@@ -668,7 +724,7 @@ int midStealToFullAWP() {
 	midSteal();
 
 	rightAWPToLeftAWP();
-	
+
 	return 0;
 }
 
@@ -1148,26 +1204,43 @@ 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();
 
 		pros::Task::delay_until(&startTime, 20);
 	}
 }
 
+void updateDisplay() {
+
+	// Odom
+    lv_obj_t* odomTab = lv_tabview_add_tab(tabview, "Odom");
+	lv_obj_t* odomLabel = lv_label_create(odomTab, NULL);
+
+	// Balance
+    lv_obj_t* balanceTab = lv_tabview_add_tab(tabview, "Balance");
+	lv_obj_t* balanceLabel = lv_label_create(balanceTab, NULL);
+
+	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());
+	}
+}
+
+void initDisplay() {
+	pros::Task updateDisplayTask = pros::Task(updateDisplay);
+}
+
 /**
  * Initialize all sensors
  */
@@ -1200,13 +1273,13 @@ void updateMotors() {
 			}
 		}
 
-		if (backButtonStatus) {
+		if (backButtonStatus && !backGrabberManual) {
 			backGrabberButton2.setButtonStatus(POSITIVE);
 			if (pros::millis() - lastChange > 100) {
 				backGrabberButton.setButtonStatus(POSITIVE);
 			}
 		}
-		else {
+		else if (!backGrabberManual) {
 			backGrabberButton.setButtonStatus(NEUTRAL);
 			if (pros::millis() - lastChange > 150) {
 				backGrabberButton2.setButtonStatus(NEUTRAL);
@@ -1368,6 +1441,7 @@ void initialize() {
 	printf("Initialize");
 
 	lv_init();
+	tabview = lv_tabview_create(lv_scr_act(), NULL);
 
 	printf("LVGL Init");
 
@@ -1378,6 +1452,7 @@ void initialize() {
 	initDrivetrain();
 	initController();
 	initLogger();
+	initDisplay();
 	// initSelector();
 
 	printf("Init done\n");
@@ -1430,7 +1505,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	skills();
+	leftStealLeftAWP();
 	postAuton();
 
 	// autonomousSelector.run();

From 77004c41e04dbd54fb11fefb883a97ad75a4621a Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Fri, 22 Apr 2022 21:40:00 -0600
Subject: [PATCH 02/13] Testing autons an a little bit of skills

---
 include/autoPaths.hpp                 |  44 ++++++----
 include/motionControl/purePursuit.hpp |   1 +
 pathTest.cpp                          |  14 ++--
 src/main.cpp                          | 116 ++++++++++++++++++++++----
 src/motionControl/purePursuit.cpp     |   3 +
 src/motionControl/tankPurePursuit.cpp |   2 +-
 src/odometry/threeWheelOdom.cpp       |  19 +++--
 7 files changed, 148 insertions(+), 51 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 701408ad..8955de7e 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -63,6 +63,7 @@ namespace Pronounce {
 
 	// Right AWP
 	int enterRightHomeZoneToRightAllianceIndex = -1;
+	int rightPlatformToRightAllianceIndex = -1;
 	int rightAllianceToRightRingsIndex = -1;
 	int rightAllianceToLeftAllianceIndex = -1;
 	int rightRingsToRightHomeZoneIndex = -1;
@@ -291,33 +292,33 @@ namespace Pronounce {
 
 		QuadraticSplinePath farRightHomeZoneToRightNeutral;
 
-		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(117, 19.5), Vector(0.0, 0)));
-		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(112, 46), Vector(0.0, 0)));
+		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
+		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
 
 		farRightHomeZoneToRightNeutralIndex = purePursuit->addPath(farRightHomeZoneToRightNeutral.getPath(0.01));
 
 		QuadraticSplinePath rightNeutralToFarRightHomeZone;
 
-		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(112, 46), Vector(0.0, 0)));
-		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(117, 19.5), Vector(0.0, 0)));
+		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
+		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
 
 		rightNeutralToFarRightHomeZoneIndex = purePursuit->addPath(rightNeutralToFarRightHomeZone.getPath(0.01));
 
 		// SECTION Mid Steal
 
-		QuadraticSplinePath rightHomeZoneToMidNeutral;
+		Path rightHomeZoneToMidNeutral;
+		// Distance: 32
+		rightHomeZoneToMidNeutral.addPoint(99.5, 18);
+		rightHomeZoneToMidNeutral.addPoint(82, 47);
 
-		rightHomeZoneToMidNeutral.addPoint(SplinePoint(Point(100, 16), Vector(0.0, 0)));
-		rightHomeZoneToMidNeutral.addPoint(SplinePoint(Point(83, 48), Vector(0.0, 0)));
+		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral);
 
-		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral.getPath(0.01));
+		Path midNeutralToRightHomeZone;
 
-		QuadraticSplinePath midNeutralToRightHomeZone;
+		midNeutralToRightHomeZone.addPoint(82, 47);
+		midNeutralToRightHomeZone.addPoint(110, 25);
 
-		midNeutralToRightHomeZone.addPoint(SplinePoint(Point(83, 48), Vector(0.0, 0)));
-		midNeutralToRightHomeZone.addPoint(SplinePoint(Point(100, 16), Vector(0.0, 0)));
-
-		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone.getPath(0.01));
+		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone);
 
 		//!SECTION
 			
@@ -367,10 +368,17 @@ namespace Pronounce {
 		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
 		
 		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(122, 37), Vector(15, - M_PI_2 - 0.4)));
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123, 37), Vector(15, - M_PI_2 - 0.4)));
 
 		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.01));
 
+		QuadraticSplinePath rightPlatformToRightAlliance;
+		
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 40), Vector(10.0, -M_PI_2)));
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(122, 35), Vector(15, - M_PI_2)));
+
+		rightPlatformToRightAllianceIndex = purePursuit->addPath(rightPlatformToRightAlliance.getPath(0.01));
+
 		QuadraticSplinePath rightAllianceToRightRings;
 
 		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 37), Vector(5, M_PI_4)));
@@ -389,10 +397,10 @@ namespace Pronounce {
 
 		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)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(122, 30), Vector(10.0, M_PI_4*0.7)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(80, 35), Vector(10.0, M_PI_2)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(38, 35), Vector(30, M_PI_2)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(32, 15), Vector(30, -M_PI_2)));
 
 		rightAllianceToLeftAllianceIndex = purePursuit->addPath(rightAllianceToLeftAlliance.getPath(0.01));
 
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/pathTest.cpp b/pathTest.cpp
index 028bcbbe..32f8878c 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,12 +276,14 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
-
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(60, 70), Vector(20, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(75, 70), Vector(20, 0)));
-
-	smoothPath = midGoalToFarPlatform.getPath(0.1);
+		QuadraticSplinePath rightAllianceToLeftAlliance;
+		
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(122, 25), Vector(10.0, M_PI_4*0.7)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(80, 46.8), Vector(10.0, M_PI_2)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(38, 46.8), Vector(30, M_PI_2)));
+		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(32, 15), Vector(30, -M_PI_2)));
+
+	smoothPath = rightAllianceToLeftAlliance.getPath(0.1);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index ea3cdbe2..8bd29ea1 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -230,10 +230,12 @@ void grabFromHook() {
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
-	purePursuit.setEnabled(true);
-	purePursuit.setFollowing(true);
+	//purePursuit.setEnabled(true);
+	//purePursuit.setFollowing(true);
 
-	turn(odometry.getPosition()->getTheta() - 0.1);
+	//turn(odometry.getPosition()->getTheta() - 0.1);
+
+	pros::Task::delay(200);
 
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
@@ -322,8 +324,11 @@ void leftSteal() {
 void midSteal() {
 	purePursuit.setSpeed(70);
 
+	double oldLookahead = purePursuit.getLookahead();
+
+	purePursuit.setLookahead(25);
+
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
 	purePursuit.setEnabled(true);
@@ -337,14 +342,21 @@ void midSteal() {
 
 	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
 
+	purePursuit.setSpeed(50);
+
 	while (odometry.getPosition()->getY() > 40) {
 		pros::Task::delay(50);
 	}
 
-	purePursuit.setSpeed(45);
+	for (int i = 5; i > 3; i--) {
+		purePursuit.setSpeed(i * 10);
+		pros::Task::delay(200);
+	}
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+
 	grabFromHook();
 }
 
@@ -370,6 +382,8 @@ void rightSteal() {
 
 	purePursuit.setCurrentPathIndex(rightNeutralToRightHomeZoneIndex);
 
+	purePursuit.setSpeed(65);
+
 	while (odometry.getPosition()->getY() > 40) {
 		pros::Task::delay(50);
 	}
@@ -387,7 +401,6 @@ void rightRightSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
 	purePursuit.setEnabled(true);
@@ -401,6 +414,8 @@ void rightRightSteal() {
 
 	purePursuit.setCurrentPathIndex(rightNeutralToFarRightHomeZoneIndex);
 
+	purePursuit.setSpeed(65);
+
 	while (odometry.getPosition()->getY() > 40) {
 		pros::Task::delay(50);
 	}
@@ -419,6 +434,10 @@ void rightMidSteal() {
 	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
+	double oldLookahead = purePursuit.getLookahead();
+
+	purePursuit.setLookahead(30);
+
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
@@ -438,6 +457,8 @@ void rightMidSteal() {
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+
 	grabFromHook();
 }
 
@@ -476,7 +497,7 @@ void rightAWP() {
 
 	backGrabberChange(true);
 
-	waitForDone(0.1, 2500);
+	waitForDone(0.1, 1000);
 
 	pros::Task::delay(100);
 
@@ -496,23 +517,38 @@ void rightAWP() {
 void rightAWPToLeftAWP() {
 	purePursuit.setSpeed(40);
 
-	purePursuit.setCurrentPathIndex(rightHomeZoneToRightAllianceIndex);
+	turn(-M_PI_2);
 
-	waitForDone(8);
+	purePursuit.setCurrentPathIndex(rightPlatformToRightAllianceIndex);
+
+	waitForDone(20, 2500);
+
+	purePursuit.setSpeed(40);
+
+	waitForDone(8, 2500);
 
 	backGrabberChange(true);
 
-	waitForDone(0.1, 3000);
+	waitForDone(0.1, 2500);
 
 	intakeButton.setButtonStatus(POSITIVE);
+	liftButton.setAutonomousAuthority(600);
 
 	purePursuit.setCurrentPathIndex(rightAllianceToLeftAllianceIndex);
 
-	waitForDone(50);
+	purePursuit.setSpeed(50);
+
+	waitForDone(70);
 
 	intakeButton.setButtonStatus(NEUTRAL);
 
-	turn(-M_PI_4);
+	backGrabberChange(false);
+
+	purePursuit.setSpeed(50);
+
+	pros::Task::delay(300);
+
+	turn(0);
 
 	waitForDone();
 
@@ -602,8 +638,16 @@ int leftStealFullAWP() {
 
 	waitForDone();
 
+	backGrabberChange(false);
+
+	pros::Task::delay(1000);
+
+	purePursuit.setFollowing(true);
+
 	rightAWP();
 
+	backGrabberChange(false);
+
 	return 0;
 }
 
@@ -657,7 +701,7 @@ int rightStealFullAWP() {
  * @return int
  */
 int rightRightStealToRightAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	odometry.reset(new Position(121, 18, toRadians(-15)));
 
 	pros::Task deployBack(deployBackGrabber);
 
@@ -674,8 +718,10 @@ int rightRightStealToRightAWP() {
  * @return int
  */
 int rightRightStealToFullAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	odometry.reset(new Position(121, 18, toRadians(-15)));
 
+	pros::Task deployBack(deployBackGrabber);
+	
 	rightRightSteal();
 
 	rightAWPToLeftAWP();
@@ -691,6 +737,8 @@ int rightRightStealToFullAWP() {
 int midStealToLeftAWP() {
 	odometry.reset(new Position(115, 19.5, 0.0));
 
+	pros::Task deployBack(deployBackGrabber);
+
 	midSteal();
 
 	rightToLeftAWP();
@@ -704,7 +752,9 @@ int midStealToLeftAWP() {
  * @return int
  */
 int midStealToRightAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	odometry.reset(new Position(99.5, 18, 25.0));
+
+	pros::Task deployBack(deployBackGrabber);
 
 	midSteal();
 
@@ -719,7 +769,9 @@ int midStealToRightAWP() {
  * @return int
  */
 int midStealToFullAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	odometry.reset(new Position(99.5, 18, toRadians(-30)));
+
+	pros::Task deployBack(deployBackGrabber);
 
 	midSteal();
 
@@ -1212,6 +1264,7 @@ void updateDrivetrain() {
 		purePursuit.update();
 		balance.update();
 
+		// Has to be in increments of 10 because of the VEX sensor update time
 		pros::Task::delay_until(&startTime, 20);
 	}
 }
@@ -1222,10 +1275,24 @@ void updateDisplay() {
     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);
+
 	while (1) {
 		// Odometry
 		lv_label_set_text(odomLabel, (odometry.getPosition()->to_string()
@@ -1234,6 +1301,19 @@ void updateDisplay() {
 		
 		// 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());
+
+		pros::Task::delay(100);
 	}
 }
 
@@ -1340,7 +1420,7 @@ void initDrivetrain() {
 	// 1.072124756
 	// Left 99.57
 	// Right 100.57
-	double turningFactor = (((101.22 / 101.50) - 1.0) / 2);
+	double turningFactor = (((100.0 / 100.0) - 1.0) / 2);
 	double tuningFactor = 1.010901883;
 	leftOdomWheel.setRadius(2.75 / 2);
 	leftOdomWheel.setTuningFactor(tuningFactor * (1 - turningFactor));
@@ -1505,7 +1585,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	leftStealLeftAWP();
+	skills();
 	postAuton();
 
 	// autonomousSelector.run();
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..699f0eab 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;
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;
 		}
 

From c1d05f1fa18480ae70daa06c08fd6a20cd92bc8c Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Sun, 24 Apr 2022 11:16:23 -0600
Subject: [PATCH 03/13] Tuning skills

---
 include/autoPaths.hpp | 38 +++++++++++++++++++-------------------
 src/main.cpp          | 22 +++++++++++++---------
 2 files changed, 32 insertions(+), 28 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 8955de7e..fd4cf028 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -108,14 +108,14 @@ namespace Pronounce {
 		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
 
 		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 72), Vector(25, -M_PI_4)));
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(73, 110), Vector(30, 0.1)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(78, 110), Vector(25, 0.1)));
 
 		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
 
 		QuadraticSplinePath farLeftHomeZoneToFarRightGoalDropOff("Left Home Zone to far right goal drop off");
 
-		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(60, 107), Vector(0.0, 0.0)));
-		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(85, 107), Vector(0.0, 0.0)));
+		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(78, 107), Vector(0.0, 0.0)));
+		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(83, 107), Vector(0.0, 0.0)));
 
 		farLeftHomeZoneToFarRightGoalDropOffIndex = purePursuit->addPath(farLeftHomeZoneToFarRightGoalDropOff.getPath(0.05));
 
@@ -126,40 +126,40 @@ namespace Pronounce {
 
 		farRightDropOffToFarLeftAllianceGoalIndex = purePursuit->addPath(farRightDropOffToFarLeftAllianceGoal.getPath(0.05));
 
-		QuadraticSplinePath farLeftAllianceToMidGoal("farRightDropOffToFarLeftAllianceGoal");
+		QuadraticSplinePath farLeftAllianceToMidGoal("farLeftAllianceToMidGoal");
 
-		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(20, 105), Vector(5, -M_PI_2)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(33, 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)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(70, 67), Vector(5.0, -M_PI_2)));
 
 		farLeftAllianceToMidGoalIndex = purePursuit->addPath(farLeftAllianceToMidGoal.getPath(0.05));
 
 		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
 
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(60, 70), Vector(0.0, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(75, 105), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 108), Vector(0.0, 0)));
 
 		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.05));
 
 		QuadraticSplinePath farPlatformToFarLeftAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
 
 		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(85, 105.7), Vector(0.0, 0)));
-		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(75, 105.7), Vector(0.0, 0)));
+		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 102), Vector(0.0, 0)));
 
 		farPlatformToFarLeftAllianceDropOffIndex = purePursuit->addPath(farPlatformToFarLeftAllianceDropOff.getPath(0.05));
 
-		QuadraticSplinePath farLeftAllianceDropOffToFarRightDropOff("farPlatformToFarLeftAllianceDropOff");
+		QuadraticSplinePath farLeftAllianceDropOffToFarRightDropOff("farLeftAllianceDropOffToFarRightDropOff");
 
 		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)));
+		farLeftAllianceDropOffToFarRightDropOff.addPoint(SplinePoint(Point(88, 105), Vector(7.0, -M_PI_2)));
 
 		farLeftAllianceDropOffToFarRightDropOffIndex = purePursuit->addPath(farLeftAllianceDropOffToFarRightDropOff.getPath(0.05));
 		
 		QuadraticSplinePath farRightDropOffToPlatform("farRightDropOffToPlatform");
 
-		farRightDropOffToPlatform.addPoint(SplinePoint(Point(92, 105.7), Vector(7.0, 0)));
-		farRightDropOffToPlatform.addPoint(SplinePoint(Point(92, 110), Vector(7.0, 0)));
+		farRightDropOffToPlatform.addPoint(SplinePoint(Point(88, 105.7), Vector(7.0, 0)));
+		farRightDropOffToPlatform.addPoint(SplinePoint(Point(88, 110), Vector(7.0, 0)));
 
 		farRightDropOffToPlatformIndex = purePursuit->addPath(farRightDropOffToPlatform.getPath(0.05));
 
@@ -173,7 +173,7 @@ namespace Pronounce {
 		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
 
 		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)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(107, 125), Vector(40.0, M_PI_4 * 1.5)));
 
 		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.05));
 
@@ -181,7 +181,7 @@ namespace Pronounce {
 
 		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)));
+		farRightAllianceToRightHomeZone.addPoint(SplinePoint(Point(125, 20), Vector(20.0, M_PI)));
 
 		farRightAllianceToRightHomeZoneIndex = purePursuit->addPath(farRightAllianceToRightHomeZone.getPath(0.05));
 
@@ -194,14 +194,14 @@ namespace Pronounce {
 
 		QuadraticSplinePath rightAllianceToRightNeutral("rightAllianceToRightNeutral");
 
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(125, 30), Vector(10.0, 0)));
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(110, 62), Vector(5.0, M_PI_4)));
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(145, 35), Vector(10.0, 0)));
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(134, 62), Vector(5.0, M_PI_4)));
 
 		rightAllianceToRightNeutralIndex = purePursuit->addPath(rightAllianceToRightNeutral.getPath(0.05));
 
 		QuadraticSplinePath rightNeutralToPlatform("rightNeutralToPlatform");
 
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(110, 62), Vector(15.0, 0)));
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(134, 62), Vector(15.0, 0)));
 		rightNeutralToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
 
 		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.05));
diff --git a/src/main.cpp b/src/main.cpp
index 8bd29ea1..3fcada11 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -43,7 +43,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, 9.0);
 
 Pronounce::TankPurePursuit purePursuit(&drivetrain, &odometry, new PID(0.6, 0, 2.0), 20);
 
@@ -844,8 +844,6 @@ int skills() {
 
 	purePursuit.setSpeed(60);
 
-	turn(-M_PI_4 * 0.66, 0.3);
-
 	turn(0, 0.1);
 
 	purePursuit.setFollowing(false);
@@ -874,7 +872,7 @@ int skills() {
 
 	purePursuit.setSpeed(60);
 
-	waitForDone(40);
+	waitForDone(30);
 
 	liftButton.setAutonomousAuthority(600);
 
@@ -898,15 +896,19 @@ int skills() {
 
 	purePursuit.setSpeed(30);
 
-	turn(M_PI_2 + M_PI_4);
+	turn(M_PI*0.9);
 
 	purePursuit.setCurrentPathIndex(farLeftAllianceToMidGoalIndex);
 
 	intakeButton.setButtonStatus(POSITIVE);
 
+	purePursuit.setSpeed(30);
+
+	waitForDone(20);
+
 	purePursuit.setSpeed(20);
 
-	waitForDone(15);
+	waitForDone(10);
 
 	liftButton.setAutonomousAuthority(0);
 
@@ -924,7 +926,7 @@ int skills() {
 
 	liftButton.setAutonomousAuthority(2000);
 
-	purePursuit.setSpeed(20);
+	purePursuit.setSpeed(40);
 
 	placeOnPlatform();
 
@@ -986,11 +988,13 @@ int skills() {
 
 	intakeButton.setButtonStatus(POSITIVE);
 
+	purePursuit.setSpeed(60);
+
 	while (odometry.getPosition()->getY() > 85) {
 		pros::Task::delay(50);
 	}
 
-	purePursuit.setSpeed(50);
+	purePursuit.setSpeed(40);
 
 	while (odometry.getPosition()->getY() > 40) {
 		pros::Task::delay(50);
@@ -998,7 +1002,7 @@ int skills() {
 
 	purePursuit.setSpeed(10);
 
-	while (odometry.getPosition()->getY() > 35) {
+	while (odometry.getPosition()->getY() > 30) {
 		pros::Task::delay(50);
 	}
 

From 15a1c960620044c96cd0824e7427843bae2d8fee Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Sun, 24 Apr 2022 14:28:39 -0600
Subject: [PATCH 04/13] working on prog

---
 include/autoPaths.hpp | 35 +++++++++++++-------------
 pathTest.cpp          | 15 ++++++-----
 src/main.cpp          | 58 +++++++++++++++++++++++++++++++++++++------
 3 files changed, 74 insertions(+), 34 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index fd4cf028..03d0a33f 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -121,7 +121,7 @@ namespace Pronounce {
 
 		QuadraticSplinePath farRightDropOffToFarLeftAllianceGoal;
 
-		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(82, 102), Vector(20.0, M_PI_2)));
+		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(82, 105), Vector(20.0, M_PI_2)));
 		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(20, 105), Vector(20.0, M_PI_2)));
 
 		farRightDropOffToFarLeftAllianceGoalIndex = purePursuit->addPath(farRightDropOffToFarLeftAllianceGoal.getPath(0.05));
@@ -131,21 +131,21 @@ namespace Pronounce {
 		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(20, 105), Vector(5, -M_PI_2)));
 		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(33, 85), Vector(15, M_PI)));
 		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(40, 75), Vector(10.0, -M_PI_2)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(70, 67), Vector(5.0, -M_PI_2)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(72, 70), Vector(5.0, -M_PI_2)));
 
 		farLeftAllianceToMidGoalIndex = purePursuit->addPath(farLeftAllianceToMidGoal.getPath(0.05));
 
 		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
 
 		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 108), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 112), Vector(0.0, 0)));
 
 		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.05));
 
 		QuadraticSplinePath farPlatformToFarLeftAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
 
 		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(85, 105.7), Vector(0.0, 0)));
-		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 102), Vector(0.0, 0)));
+		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 100), Vector(0.0, 0)));
 
 		farPlatformToFarLeftAllianceDropOffIndex = purePursuit->addPath(farPlatformToFarLeftAllianceDropOff.getPath(0.05));
 
@@ -173,7 +173,7 @@ namespace Pronounce {
 		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
 
 		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 105.7), Vector(20.0, -M_PI_2)));
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(107, 125), Vector(40.0, M_PI_4 * 1.5)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(105, 128), Vector(35, M_PI_2)));
 
 		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.05));
 
@@ -202,51 +202,50 @@ namespace Pronounce {
 		QuadraticSplinePath rightNeutralToPlatform("rightNeutralToPlatform");
 
 		rightNeutralToPlatform.addPoint(SplinePoint(Point(134, 62), Vector(15.0, 0)));
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(110, 115), Vector(15.0, 0)));
 
 		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.05));
 
 		QuadraticSplinePath rightPlatformToFarRightGoalDropOff2("rightPlatformToFarRightGoalDropOff2");
 
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 115), Vector(15.0, M_PI)));
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 115), Vector(15.0, M_PI)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI)));
 
 		rightPlatformToFarRightGoalDropOff2Index = purePursuit->addPath(rightPlatformToFarRightGoalDropOff2.getPath(0.05));
 
 		QuadraticSplinePath farRightGoalDropOff2ToFarLeftAllianceDropOff("farRightGoalDropOff2ToFarLeftAllianceDropOff");
 
+		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI_2)));
 		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)));
 
 		farRightGoalDropOff2ToFarLeftAllianceDropOffIndex = purePursuit->addPath(farRightGoalDropOff2ToFarLeftAllianceDropOff.getPath(0.05));
 
 		QuadraticSplinePath farLeftAllianceDropOffToFarRightGoalDropOff2("farLeftAllianceDropOffToFarRightGoalDropOff2");
 
-		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)));
+		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, -M_PI_2)));
 
 		farLeftAllianceDropOffToFarRightGoalDropOff2Index = purePursuit->addPath(farLeftAllianceDropOffToFarRightGoalDropOff2.getPath(0.05));
 
 		QuadraticSplinePath farLeftAllianceDropOffToPlatform("farLeftAllianceDropOffToPlatform");
 
-		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(60, 105.7), Vector(15.0, 0)));
-		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(60, 115), Vector(15.0, 0)));
+		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, 0)));
+		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
 
 		farLeftAllianceDropOffToPlatformIndex = purePursuit->addPath(farLeftAllianceDropOffToPlatform.getPath(0.05));
 
 		QuadraticSplinePath platformToEnterFarLeftHomeZone("platformToEnterFarLeftHomeZone");
 
-		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(60, 115), Vector(15.0, M_PI)));
-		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(60, 105.7), Vector(15.0, M_PI)));
+		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 115), Vector(15.0, M_PI)));
+		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI)));
 
 		platformToEnterFarLeftHomeZoneIndex = purePursuit->addPath(platformToEnterFarLeftHomeZone.getPath(0.05));
 
 		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		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)));
+		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)));
 
 		enterFarLeftHomeZoneToNearRightPlatformIndex = purePursuit->addPath(enterFarLeftHomeZoneToNearRightPlatform.getPath(0.05));
 
diff --git a/pathTest.cpp b/pathTest.cpp
index 32f8878c..d65f5e3b 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,14 +276,13 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath rightAllianceToLeftAlliance;
-		
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(122, 25), Vector(10.0, M_PI_4*0.7)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(80, 46.8), Vector(10.0, M_PI_2)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(38, 46.8), Vector(30, M_PI_2)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(32, 15), Vector(30, -M_PI_2)));
-
-	smoothPath = rightAllianceToLeftAlliance.getPath(0.1);
+		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
+
+		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)));
+
+	smoothPath = enterFarLeftHomeZoneToNearRightPlatform.getPath(0.1);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index 3fcada11..731629d2 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -866,7 +866,7 @@ int skills() {
 
 	purePursuit.setSpeed(60);
 
-	turn(M_PI_2 + M_PI_4);
+	turn(M_PI_2);
 
 	purePursuit.setCurrentPathIndex(farRightDropOffToFarLeftAllianceGoalIndex);
 
@@ -888,7 +888,7 @@ int skills() {
 
 	purePursuit.setSpeed(20);
 
-	waitForDone(0.5, 2000);
+	waitForDone(0.5, 1500);
 
 	backGrabberChange(true);
 
@@ -896,7 +896,7 @@ int skills() {
 
 	purePursuit.setSpeed(30);
 
-	turn(M_PI*0.9);
+	turn(M_PI*0.9, 0.3);
 
 	purePursuit.setCurrentPathIndex(farLeftAllianceToMidGoalIndex);
 
@@ -904,7 +904,7 @@ int skills() {
 
 	purePursuit.setSpeed(30);
 
-	waitForDone(20);
+	waitForDone(30);
 
 	purePursuit.setSpeed(20);
 
@@ -988,6 +988,8 @@ int skills() {
 
 	intakeButton.setButtonStatus(POSITIVE);
 
+	liftButton.setAutonomousAuthority(600);
+
 	purePursuit.setSpeed(60);
 
 	while (odometry.getPosition()->getY() > 85) {
@@ -1030,6 +1032,10 @@ int skills() {
 
 	purePursuit.setCurrentPathIndex(rightAllianceToRightNeutralIndex);
 
+	waitForDone(10);
+
+	liftButton.setAutonomousAuthority(0);
+
 	waitForDone();
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
@@ -1091,9 +1097,11 @@ int skills() {
 
 	purePursuit.setCurrentPathIndex(enterFarLeftHomeZoneToNearRightPlatformIndex);
 
+	purePursuit.setSpeed(60);
+
 	liftButton.setAutonomousAuthority(600);
 
-	while (odometry.getPosition()->getY() > 40) {
+	while (odometry.getPosition()->getY() > 45) {
 		pros::Task::delay(50);
 	}
 
@@ -1109,9 +1117,9 @@ int skills() {
 
 	pros::Task::delay(100);
 
-	liftButton.setAutonomousAuthority(1600);
+	liftButton.setAutonomousAuthority(2000);
 
-	purePursuit.setSpeed(40);
+	purePursuit.setSpeed(60);
 
 	waitForDone();
 
@@ -1273,6 +1281,20 @@ void updateDrivetrain() {
 	}
 }
 
+void addValue(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
@@ -1297,6 +1319,20 @@ void updateDisplay() {
 	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);
+
+	//chartInit(intakeInputSpeed, 50);
+	//chartInit(intakeOutputSpeed, 50);
+//
+	//pros::Task::delay(100);
+
 	while (1) {
 		// Odometry
 		lv_label_set_text(odomLabel, (odometry.getPosition()->to_string()
@@ -1317,12 +1353,18 @@ void updateDisplay() {
 		// 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_chart_refresh(intakeChart);
+
 		pros::Task::delay(100);
 	}
 }
 
 void initDisplay() {
-	pros::Task updateDisplayTask = pros::Task(updateDisplay);
+	pros::Task updateDisplayTask = pros::Task(updateDisplay, "Update dispay");
 }
 
 /**

From ae0b79adfa0cd8c187231bdfba30f4c1abecc483 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Mon, 25 Apr 2022 17:01:19 -0600
Subject: [PATCH 05/13] Tuning autons

---
 include/autoPaths.hpp | 10 ++++------
 pathTest.cpp          |  9 ++++-----
 src/main.cpp          | 14 ++++++++------
 3 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 03d0a33f..20e2af18 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -128,10 +128,8 @@ namespace Pronounce {
 
 		QuadraticSplinePath farLeftAllianceToMidGoal("farLeftAllianceToMidGoal");
 
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(20, 105), Vector(5, -M_PI_2)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(33, 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)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(20, 105), Vector(5.0, -M_PI_2)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(72, 70), Vector(20, M_PI_2-M_PI_4)));
 
 		farLeftAllianceToMidGoalIndex = purePursuit->addPath(farLeftAllianceToMidGoal.getPath(0.05));
 
@@ -254,9 +252,9 @@ namespace Pronounce {
 		// SECTION Left Steal
 
 		Path leftHomeZoneToLeftNeutralSteal;
-
+		// 25 in
 		leftHomeZoneToLeftNeutralSteal.addPoint(27, 18);
-		leftHomeZoneToLeftNeutralSteal.addPoint(33, 48);
+		leftHomeZoneToLeftNeutralSteal.addPoint(31, 42.6);
 
 		leftHomeZoneToLeftNeutralStealIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralSteal);
 
diff --git a/pathTest.cpp b/pathTest.cpp
index d65f5e3b..84f74fcb 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,13 +276,12 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
+		QuadraticSplinePath farLeftAllianceToMidGoal("farLeftAllianceToMidGoal");
 
-		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)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(20, 105), Vector(20, -M_PI_2)));
+		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(72, 70), Vector(20, M_PI_2-M_PI_4)));
 
-	smoothPath = enterFarLeftHomeZoneToNearRightPlatform.getPath(0.1);
+	smoothPath = farLeftAllianceToMidGoal.getPath(0.1);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index 731629d2..c15b1e44 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -308,6 +308,8 @@ void leftSteal() {
 
 	purePursuit.setCurrentPathIndex(leftNeutralStealToLeftHomeZoneIndex);
 
+	purePursuit.setSpeed(60);
+
 	while (odometry.getPosition()->getY() > 40) {
 		pros::Task::delay(50);
 	}
@@ -597,7 +599,7 @@ void rightToLeftAWP() {
  * @return int
  */
 int leftStealLeftAWP() {
-	odometry.reset(new Position(27, 18, 0.17));
+	odometry.reset(new Position(27, 18, toRadians(10)));
 
 	pros::Task deployBack(deployBackGrabber);
 
@@ -624,7 +626,7 @@ int leftStealLeftAWP() {
  * @return int
  */
 int leftStealFullAWP() {
-	odometry.reset(new Position(27, 18, 0.17));
+	odometry.reset(new Position(27, 18, toRadians(10)));
 
 	leftSteal();
 
@@ -1281,7 +1283,7 @@ void updateDrivetrain() {
 	}
 }
 
-void addValue(lv_chart_series_t* chartSeries, int size, int value) {
+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];
 	}
@@ -1466,8 +1468,8 @@ void initDrivetrain() {
 	// 1.072124756
 	// Left 99.57
 	// Right 100.57
-	double turningFactor = (((100.0 / 100.0) - 1.0) / 2);
-	double tuningFactor = 1.010901883;
+	double turningFactor = (((100.79 / 100.35) - 1.0) / 2);
+	double tuningFactor = 0.998791278;
 	leftOdomWheel.setRadius(2.75 / 2);
 	leftOdomWheel.setTuningFactor(tuningFactor * (1 - turningFactor));
 	rightOdomWheel.setRadius(2.75 / 2);
@@ -1631,7 +1633,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	skills();
+	rightStealRightAWP();
 	postAuton();
 
 	// autonomousSelector.run();

From 19af4ffaab7026ee1a9a4b5c02ac90cf0b60db86 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Thu, 28 Apr 2022 12:53:55 -0600
Subject: [PATCH 06/13] Working on optimising autons

---
 include/autoPaths.hpp                      | 10 +--
 include/chassis/abstractTankDrivetrain.hpp | 17 ++++
 include/chassis/tankDrive.hpp              |  5 ++
 include/motionControl/tankPurePursuit.hpp  | 10 +++
 src/main.cpp                               | 91 ++++++++++++++++------
 src/motionControl/tankPurePursuit.cpp      | 11 ++-
 6 files changed, 113 insertions(+), 31 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 20e2af18..ca6004ee 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -272,13 +272,13 @@ namespace Pronounce {
 		QuadraticSplinePath rightHomeZoneToRightNeutral;
 
 		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 46), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 42), Vector(0.0, 0)));
 
 		rightHomeZoneToRightNeutralIndex = purePursuit->addPath(rightHomeZoneToRightNeutral.getPath(0.01));
 
 		QuadraticSplinePath rightNeutralToRightHomeZone;
 
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 46), Vector(0.0, 0)));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 42), Vector(0.0, 0)));
 		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 17), Vector(0.0, 0)));
 
 		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.01));
@@ -306,7 +306,7 @@ namespace Pronounce {
 		Path rightHomeZoneToMidNeutral;
 		// Distance: 32
 		rightHomeZoneToMidNeutral.addPoint(99.5, 18);
-		rightHomeZoneToMidNeutral.addPoint(82, 47);
+		rightHomeZoneToMidNeutral.addPoint(99.5+(sin(toRadians(-30)) * 30), 47+(cos(toRadians(-30)) * 30));
 
 		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral);
 
@@ -365,14 +365,14 @@ namespace Pronounce {
 		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
 		
 		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123, 37), Vector(15, - M_PI_2 - 0.4)));
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123, 35), Vector(15, - M_PI_2 - 0.4)));
 
 		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.01));
 
 		QuadraticSplinePath rightPlatformToRightAlliance;
 		
 		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 40), Vector(10.0, -M_PI_2)));
-		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(122, 35), Vector(15, - M_PI_2)));
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(122, 35), Vector(15, -M_PI_2)));
 
 		rightPlatformToRightAllianceIndex = purePursuit->addPath(rightPlatformToRightAlliance.getPath(0.01));
 
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..5c26cb0f 100644
--- a/include/chassis/tankDrive.hpp
+++ b/include/chassis/tankDrive.hpp
@@ -29,6 +29,11 @@ namespace Pronounce {
 			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/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/src/main.cpp b/src/main.cpp
index c15b1e44..063f0d29 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -57,6 +57,8 @@ SolenoidButton frontHookButton(&master, DIGITAL_X);
 SolenoidButton backGrabberButton(&master, DIGITAL_R1, DIGITAL_R1);
 SolenoidButton backGrabberButton2(&master, DIGITAL_R1, DIGITAL_R1);
 
+pros::Vision clawVision(18, VISION_ZERO_CENTER);
+
 // Autonomous Selector
 Pronounce::AutonSelector autonomousSelector;
 
@@ -84,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.0035 * (double) x + 0.02;
+
+	return angle;
+}
+
 void waitForDone(double distance, double timeout) {
 	uint32_t startTime = pros::millis();
 
@@ -218,6 +232,10 @@ void grabFromHook() {
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
 
+	pros::Task::delay(300);
+
+	double angle = goalAngle();
+
 	frontHookButton.setButtonStatus(POSITIVE);
 
 	pros::Task::delay(200);
@@ -230,10 +248,10 @@ void grabFromHook() {
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
-	//purePursuit.setEnabled(true);
-	//purePursuit.setFollowing(true);
+	purePursuit.setEnabled(true);
+	purePursuit.setFollowing(true);
 
-	//turn(odometry.getPosition()->getTheta() - 0.1);
+	turn(odometry.getPosition()->getTheta() + angle);
 
 	pros::Task::delay(200);
 
@@ -373,6 +391,8 @@ void rightSteal() {
 
 	purePursuit.setLookahead(20);
 
+	purePursuit.setUseVoltage(true);
+	
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
@@ -390,6 +410,8 @@ void rightSteal() {
 		pros::Task::delay(50);
 	}
 
+	purePursuit.setUseVoltage(false);
+
 	purePursuit.setSpeed(45);
 
 	waitForDone();
@@ -495,18 +517,18 @@ void rightAWP() {
 
 	purePursuit.setSpeed(25);
 
-	waitForDone(8, 2500);
+	waitForDone(0.1, 1000);
 
 	backGrabberChange(true);
 
-	waitForDone(0.1, 1000);
-
 	pros::Task::delay(100);
 
 	purePursuit.setCurrentPathIndex(rightAllianceToRightRingsIndex);
 
 	liftButton.setAutonomousAuthority(2000);
 
+	pros::Task::delay(300);
+
 	intakeButton.setButtonStatus(POSITIVE);
 
 	waitForDone(0.1, 3000);
@@ -723,7 +745,7 @@ int rightRightStealToFullAWP() {
 	odometry.reset(new Position(121, 18, toRadians(-15)));
 
 	pros::Task deployBack(deployBackGrabber);
-	
+
 	rightRightSteal();
 
 	rightAWPToLeftAWP();
@@ -898,7 +920,7 @@ int skills() {
 
 	purePursuit.setSpeed(30);
 
-	turn(M_PI*0.9, 0.3);
+	turn(M_PI * 0.9, 0.3);
 
 	purePursuit.setCurrentPathIndex(farLeftAllianceToMidGoalIndex);
 
@@ -1284,11 +1306,11 @@ void updateDrivetrain() {
 }
 
 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];
+	for (int i = 0; i < size - 1; i++) {
+		chartSeries->points[i] = chartSeries->points[i + 1];
 	}
 
-	chartSeries->points[size-1] = value;
+	chartSeries->points[size - 1] = value;
 }
 
 void chartInit(lv_chart_series_t* chartSeries, int size) {
@@ -1300,19 +1322,19 @@ void chartInit(lv_chart_series_t* chartSeries, int size) {
 void updateDisplay() {
 
 	// Odom
-    lv_obj_t* odomTab = lv_tabview_add_tab(tabview, "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* 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* 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* drivetrainTab = lv_tabview_add_tab(tabview, "Drivetrain");
 	lv_obj_t* drivetrainTable = lv_table_create(drivetrainTab, NULL);
 
 	lv_table_set_row_cnt(drivetrainTable, 3);
@@ -1324,11 +1346,16 @@ void updateDisplay() {
 	// 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_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);
+	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);
@@ -1338,9 +1365,9 @@ void updateDisplay() {
 	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());
-		
+			+ "\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());
 
@@ -1359,6 +1386,8 @@ void updateDisplay() {
 		//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);
@@ -1545,6 +1574,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.
  */
@@ -1581,6 +1617,7 @@ void initialize() {
 	initController();
 	initLogger();
 	initDisplay();
+	initVision();
 	// initSelector();
 
 	printf("Init done\n");
@@ -1633,7 +1670,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	rightStealRightAWP();
+	midStealToFullAWP();
 	postAuton();
 
 	// autonomousSelector.run();
@@ -1737,6 +1774,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/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp
index 699f0eab..d4caa2fb 100644
--- a/src/motionControl/tankPurePursuit.cpp
+++ b/src/motionControl/tankPurePursuit.cpp
@@ -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(clamp(motorSpeed * (12000/200.0), -12000.0, 12000.0), pointData.curvature);
+		} else {
+			drivetrain->driveCurvature(motorSpeed, pointData.curvature);
+		}
 	}
 
 	void TankPurePursuit::stop() {

From 48e10b218214cfeefa8a502a519830a7c308e9b6 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Thu, 28 Apr 2022 12:54:33 -0600
Subject: [PATCH 07/13] Fix minor error

---
 include/autoPaths.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index ca6004ee..47247b44 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -306,7 +306,7 @@ namespace Pronounce {
 		Path rightHomeZoneToMidNeutral;
 		// Distance: 32
 		rightHomeZoneToMidNeutral.addPoint(99.5, 18);
-		rightHomeZoneToMidNeutral.addPoint(99.5+(sin(toRadians(-30)) * 30), 47+(cos(toRadians(-30)) * 30));
+		rightHomeZoneToMidNeutral.addPoint(99.5+(sin(toRadians(-30)) * 30), 18+(cos(toRadians(-30)) * 30));
 
 		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral);
 

From f74812df7ff71a0d7b06399acea299a6168011bb Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Sat, 30 Apr 2022 18:14:12 -0600
Subject: [PATCH 08/13] Working on autons

---
 include/autoPaths.hpp                 | 386 +++++++++----------
 include/chassis/tankDrive.hpp         |   5 +
 pathTest.cpp                          |   9 +-
 src/main.cpp                          | 525 +++++++++++++-------------
 src/motionControl/tankPurePursuit.cpp |   2 +-
 5 files changed, 452 insertions(+), 475 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 47247b44..8ce24e1e 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -38,6 +38,11 @@ namespace Pronounce {
 	int platformToEnterFarLeftHomeZoneIndex = -1;
 	int enterFarLeftHomeZoneToNearRightPlatformIndex = -1;
 
+	int farPlatformToEnterFarHomeZoneIndex = -1;
+	int enterFarHomeZoneToMidGoalIndex = -1;
+	int farPlatformToRightAllianceDropOffIndex = -1;
+	int rightAllianceToFarPlatformIndex = -1;
+
 	// Left Steal
 	int leftHomeZoneToLeftNeutralStealIndex = -1;
 	int leftNeutralStealToLeftHomeZoneIndex = -1;
@@ -58,8 +63,7 @@ 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;
@@ -95,322 +99,290 @@ namespace Pronounce {
 
 		// !SECTION
 
-		//SECTION Skills
-
-		// Left home zone to left neutral goal
-		QuadraticSplinePath leftHomeZoneToLeftNeutralGoal("Left Home Zone to Left Neutral Goal");
+		// SECTION Left Steal
 
-		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(26.5, 11), Vector(20, M_PI_2)));
-		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(38, 67), Vector(10, 0)));
+		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);
 
-		leftHomeZoneToLeftNeutralGoalIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.05));
+		leftHomeZoneToLeftNeutralStealIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralSteal);
 
-		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
+		Path leftNeutralStealToLeftHomeZone;
 
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 72), Vector(25, -M_PI_4)));
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(78, 110), Vector(25, 0.1)));
+		leftNeutralStealToLeftHomeZone.addPoint(27+sin(leftAngle) * leftDistance, 18 + cos(leftAngle)*leftDistance);
+		leftNeutralStealToLeftHomeZone.addPoint(25, 18);
 
-		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
+		leftNeutralStealToLeftHomeZoneIndex = purePursuit->addPath(leftNeutralStealToLeftHomeZone);
 
-		QuadraticSplinePath farLeftHomeZoneToFarRightGoalDropOff("Left Home Zone to far right goal drop off");
+		// !SECTION
 
-		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(78, 107), Vector(0.0, 0.0)));
-		farLeftHomeZoneToFarRightGoalDropOff.addPoint(SplinePoint(Point(83, 107), Vector(0.0, 0.0)));
+		// SECTION Right steal
 
-		farLeftHomeZoneToFarRightGoalDropOffIndex = purePursuit->addPath(farLeftHomeZoneToFarRightGoalDropOff.getPath(0.05));
+		QuadraticSplinePath rightHomeZoneToRightNeutral;
 
-		QuadraticSplinePath farRightDropOffToFarLeftAllianceGoal;
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 43), Vector(0.0, 0)));
 
-		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(82, 105), Vector(20.0, M_PI_2)));
-		farRightDropOffToFarLeftAllianceGoal.addPoint(SplinePoint(Point(20, 105), Vector(20.0, M_PI_2)));
+		rightHomeZoneToRightNeutralIndex = purePursuit->addPath(rightHomeZoneToRightNeutral.getPath(0.1));
 
-		farRightDropOffToFarLeftAllianceGoalIndex = purePursuit->addPath(farRightDropOffToFarLeftAllianceGoal.getPath(0.05));
+		QuadraticSplinePath rightNeutralToRightHomeZone;
 
-		QuadraticSplinePath farLeftAllianceToMidGoal("farLeftAllianceToMidGoal");
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 43), Vector(0.0, 0)));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 17), Vector(0.0, 0)));
 
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(20, 105), Vector(5.0, -M_PI_2)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(72, 70), Vector(20, M_PI_2-M_PI_4)));
+		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.1));
 
-		farLeftAllianceToMidGoalIndex = purePursuit->addPath(farLeftAllianceToMidGoal.getPath(0.05));
+		// !SECTION
 
-		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
+		// SECTION Right right steal
 
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 112), Vector(0.0, 0)));
+		QuadraticSplinePath farRightHomeZoneToRightNeutral;
 
-		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.05));
+		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
+		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
 
-		QuadraticSplinePath farPlatformToFarLeftAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
+		farRightHomeZoneToRightNeutralIndex = purePursuit->addPath(farRightHomeZoneToRightNeutral.getPath(0.1));
 
-		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(85, 105.7), Vector(0.0, 0)));
-		farPlatformToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 100), Vector(0.0, 0)));
+		QuadraticSplinePath rightNeutralToFarRightHomeZone;
 
-		farPlatformToFarLeftAllianceDropOffIndex = purePursuit->addPath(farPlatformToFarLeftAllianceDropOff.getPath(0.05));
+		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
+		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
 
-		QuadraticSplinePath farLeftAllianceDropOffToFarRightDropOff("farLeftAllianceDropOffToFarRightDropOff");
+		rightNeutralToFarRightHomeZoneIndex = purePursuit->addPath(rightNeutralToFarRightHomeZone.getPath(0.1));
 
-		farLeftAllianceDropOffToFarRightDropOff.addPoint(SplinePoint(Point(58.9, 105.7), Vector(7.0, -M_PI_2)));
-		farLeftAllianceDropOffToFarRightDropOff.addPoint(SplinePoint(Point(88, 105), Vector(7.0, -M_PI_2)));
+		// SECTION Mid Steal
 
-		farLeftAllianceDropOffToFarRightDropOffIndex = purePursuit->addPath(farLeftAllianceDropOffToFarRightDropOff.getPath(0.05));
+		double midGoalDistance = 32.66;
+		double angle = -30;
 		
-		QuadraticSplinePath farRightDropOffToPlatform("farRightDropOffToPlatform");
-
-		farRightDropOffToPlatform.addPoint(SplinePoint(Point(88, 105.7), Vector(7.0, 0)));
-		farRightDropOffToPlatform.addPoint(SplinePoint(Point(88, 110), Vector(7.0, 0)));
-
-		farRightDropOffToPlatformIndex = purePursuit->addPath(farRightDropOffToPlatform.getPath(0.05));
-
-		QuadraticSplinePath platformToEnterFarRightHomeZone("platformToEnterFarRightHomeZone");
-
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 115), Vector(7.0, M_PI)));
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 105.7), Vector(7.0, M_PI)));
-
-		platformToEnterFarRightHomeZoneIndex = purePursuit->addPath(platformToEnterFarRightHomeZone.getPath(0.05));
-
-		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
-
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 105.7), Vector(20.0, -M_PI_2)));
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(105, 128), Vector(35, M_PI_2)));
-
-		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.05));
-
-		QuadraticSplinePath farRightAllianceToRightHomeZone("farRightAllianceToRightHomeZone");
+		Path rightHomeZoneToMidNeutral;
+		// Distance: 32
+		rightHomeZoneToMidNeutral.addPoint(99.5, 18);
+		rightHomeZoneToMidNeutral.addPoint(99.5+(sin(toRadians(angle)) * midGoalDistance), 18+(cos(toRadians(angle)) * midGoalDistance));
 
-		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, 20), Vector(20.0, M_PI)));
+		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral);
 
-		farRightAllianceToRightHomeZoneIndex = purePursuit->addPath(farRightAllianceToRightHomeZone.getPath(0.05));
+		Path midNeutralToRightHomeZone;
 
-		QuadraticSplinePath rightHomeZoneToRightAlliance("rightHomeZoneToRightAlliance");
+		midNeutralToRightHomeZone.addPoint(99.5+(sin(toRadians(angle)) * midGoalDistance), 18+(cos(toRadians(angle)) * midGoalDistance));
+		midNeutralToRightHomeZone.addPoint(107, 50);
+		midNeutralToRightHomeZone.addPoint(112, 25);
 
-		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)));
+		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone);
 
-		rightHomeZoneToRightAllianceIndex = purePursuit->addPath(rightHomeZoneToRightAlliance.getPath(0.05));
+		//!SECTION
+			
+		// SECTION Left AWP
+		
+		QuadraticSplinePath leftHomeZoneToLeftAlliance;
+		
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 30), Vector(10.0, M_PI)));
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(40, 15), Vector(20, M_PI + M_PI_4)));
 
-		QuadraticSplinePath rightAllianceToRightNeutral("rightAllianceToRightNeutral");
+		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.1));
 
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(145, 35), Vector(10.0, 0)));
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(134, 62), Vector(5.0, M_PI_4)));
+		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)));
 
-		rightAllianceToRightNeutralIndex = purePursuit->addPath(rightAllianceToRightNeutral.getPath(0.05));
+		leftAllianceToRightHomeZoneIndex = purePursuit->addPath(leftAllianceToRightHomeZone.getPath(0.1));
 
-		QuadraticSplinePath rightNeutralToPlatform("rightNeutralToPlatform");
+		QuadraticSplinePath rightHomeZoneToLeftHomeZone;
 
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(134, 62), Vector(15.0, 0)));
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(110, 115), Vector(15.0, 0)));
+		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(110, 34), Vector(0.0, M_PI_2)));
+		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(25, 34), Vector(0.0, M_PI_2)));
 
-		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.05));
+		rightHomeZoneToLeftHomeZoneIndex = purePursuit->addPath(rightHomeZoneToLeftHomeZone.getPath(0.2));
 
-		QuadraticSplinePath rightPlatformToFarRightGoalDropOff2("rightPlatformToFarRightGoalDropOff2");
+		QuadraticSplinePath rightHomeZoneToLeftAlliance;
 
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 115), Vector(15.0, M_PI)));
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI)));
+		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));
 
-		rightPlatformToFarRightGoalDropOff2Index = purePursuit->addPath(rightPlatformToFarRightGoalDropOff2.getPath(0.05));
+		// !SECTION
 
-		QuadraticSplinePath farRightGoalDropOff2ToFarLeftAllianceDropOff("farRightGoalDropOff2ToFarLeftAllianceDropOff");
+		// SECTION Right AWP
 
-		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI_2)));
-		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI_2)));
+		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
+		
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(127, 32), Vector(15, -M_PI_2)));
 
-		farRightGoalDropOff2ToFarLeftAllianceDropOffIndex = purePursuit->addPath(farRightGoalDropOff2ToFarLeftAllianceDropOff.getPath(0.05));
+		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.1));
 
-		QuadraticSplinePath farLeftAllianceDropOffToFarRightGoalDropOff2("farLeftAllianceDropOffToFarRightGoalDropOff2");
+		QuadraticSplinePath rightPlatformToRightAlliance;
+		
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 40), Vector(10.0, -M_PI_2)));
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(125, 35), Vector(15, -M_PI_2)));
 
-		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, -M_PI_2)));
-		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, -M_PI_2)));
+		rightPlatformToRightAllianceIndex = purePursuit->addPath(rightPlatformToRightAlliance.getPath(0.1));
 
-		farLeftAllianceDropOffToFarRightGoalDropOff2Index = purePursuit->addPath(farLeftAllianceDropOffToFarRightGoalDropOff2.getPath(0.05));
+		QuadraticSplinePath rightAllianceToRightRings;
 
-		QuadraticSplinePath farLeftAllianceDropOffToPlatform("farLeftAllianceDropOffToPlatform");
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 37), Vector(5, M_PI_4)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 55), Vector(10.0, 0)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 60), Vector(10.0, 0)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(125, 70), Vector(10.0, -M_PI_2)));
 
-		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, 0)));
-		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
+		rightAllianceToRightRingsIndex = purePursuit->addPath(rightAllianceToRightRings.getPath(0.1));
 
-		farLeftAllianceDropOffToPlatformIndex = purePursuit->addPath(farLeftAllianceDropOffToPlatform.getPath(0.05));
+		QuadraticSplinePath rightRingsToRightHomeZone;
 
-		QuadraticSplinePath platformToEnterFarLeftHomeZone("platformToEnterFarLeftHomeZone");
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(20.0, M_PI_2)));
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(120, 25), Vector(15, M_PI)));
 
-		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 115), Vector(15.0, M_PI)));
-		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI)));
+		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.1));
 
-		platformToEnterFarLeftHomeZoneIndex = purePursuit->addPath(platformToEnterFarLeftHomeZone.getPath(0.05));
+		// !SECTION
 
-		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		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)));
+		//SECTION Skills
 
-		enterFarLeftHomeZoneToNearRightPlatformIndex = purePursuit->addPath(enterFarLeftHomeZoneToNearRightPlatform.getPath(0.05));
+		// Left home zone to left neutral goal
+		QuadraticSplinePath leftHomeZoneToLeftNeutralGoal("Left Home Zone to Left Neutral Goal");
 
-		// !SECTION
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(26.5, 11), Vector(20, M_PI_2)));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(38, 67), Vector(10, 0)));
 
-		// SECTION Left Steal
+		leftHomeZoneToLeftNeutralGoalIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.1));
 
-		Path leftHomeZoneToLeftNeutralSteal;
-		// 25 in
-		leftHomeZoneToLeftNeutralSteal.addPoint(27, 18);
-		leftHomeZoneToLeftNeutralSteal.addPoint(31, 42.6);
+		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
 
-		leftHomeZoneToLeftNeutralStealIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralSteal);
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 72), Vector(25, -M_PI_4)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(78, 110), Vector(25, 0.1)));
 
-		Path leftNeutralStealToLeftHomeZone;
+		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
 
-		leftNeutralStealToLeftHomeZone.addPoint(33, 48);
-		leftNeutralStealToLeftHomeZone.addPoint(25, 18);
+		QuadraticSplinePath farPlatformToEnterFarHomeZone("Left Home Zone to far right goal drop off");
 
-		leftNeutralStealToLeftHomeZoneIndex = purePursuit->addPath(leftNeutralStealToLeftHomeZone);
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(78, 107), Vector(0.0, 0.0)));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(78, 95), Vector(0.0, 0.0)));
 
-		// !SECTION
+		farPlatformToEnterFarHomeZoneIndex = purePursuit->addPath(farPlatformToEnterFarHomeZone.getPath(0.1));
 
-		// SECTION Right steal
+		QuadraticSplinePath enterFarHomeZoneToMidGoal("farLeftAllianceToMidGoal");
 
-		QuadraticSplinePath rightHomeZoneToRightNeutral;
+		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(78, 95), Vector(5.0, -M_PI)));
+		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(70, 70), Vector(10, -M_PI)));
 
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
-		rightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(107, 42), Vector(0.0, 0)));
+		enterFarHomeZoneToMidGoalIndex = purePursuit->addPath(enterFarHomeZoneToMidGoal.getPath(0.2));
 
-		rightHomeZoneToRightNeutralIndex = purePursuit->addPath(rightHomeZoneToRightNeutral.getPath(0.01));
+		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
 
-		QuadraticSplinePath rightNeutralToRightHomeZone;
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(85, 107), Vector(0.0, 0)));
 
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 42), Vector(0.0, 0)));
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 17), Vector(0.0, 0)));
+		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.2));
 
-		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.01));
+		QuadraticSplinePath farPlatformToRightAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
 
-		// !SECTION
+		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 107), Vector(0.0, 0)));
+		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 90), Vector(0.0, 0)));
 
-		// SECTION Right right steal
+		farPlatformToRightAllianceDropOffIndex = purePursuit->addPath(farPlatformToRightAllianceDropOff.getPath(0.1));
+		
+		QuadraticSplinePath rightAllianceToFarPlatform("farRightDropOffToPlatform");
 
-		QuadraticSplinePath farRightHomeZoneToRightNeutral;
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(80, 90), Vector(7.0, 0)));
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(85, 107), Vector(7.0, 0)));
 
-		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
-		farRightHomeZoneToRightNeutral.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
+		rightAllianceToFarPlatformIndex = purePursuit->addPath(rightAllianceToFarPlatform.getPath(0.1));
 
-		farRightHomeZoneToRightNeutralIndex = purePursuit->addPath(farRightHomeZoneToRightNeutral.getPath(0.01));
+		QuadraticSplinePath platformToEnterFarRightHomeZone("platformToEnterFarRightHomeZone");
 
-		QuadraticSplinePath rightNeutralToFarRightHomeZone;
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 115), Vector(7.0, M_PI)));
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 105.7), Vector(7.0, M_PI)));
 
-		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(113, 47), Vector(0.0, 0)));
-		rightNeutralToFarRightHomeZone.addPoint(SplinePoint(Point(121, 18), Vector(0.0, 0)));
+		platformToEnterFarRightHomeZoneIndex = purePursuit->addPath(platformToEnterFarRightHomeZone.getPath(0.1));
 
-		rightNeutralToFarRightHomeZoneIndex = purePursuit->addPath(rightNeutralToFarRightHomeZone.getPath(0.01));
+		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
 
-		// SECTION Mid Steal
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 105.7), Vector(20.0, -M_PI_2)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(105, 122), Vector(35, M_PI_2)));
 
-		Path rightHomeZoneToMidNeutral;
-		// Distance: 32
-		rightHomeZoneToMidNeutral.addPoint(99.5, 18);
-		rightHomeZoneToMidNeutral.addPoint(99.5+(sin(toRadians(-30)) * 30), 18+(cos(toRadians(-30)) * 30));
+		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.1));
 
-		rightHomeZoneToMidNeutralIndex = purePursuit->addPath(rightHomeZoneToMidNeutral);
+		QuadraticSplinePath farRightAllianceToRightHomeZone("farRightAllianceToRightHomeZone");
 
-		Path midNeutralToRightHomeZone;
+		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, 20), Vector(20.0, M_PI)));
 
-		midNeutralToRightHomeZone.addPoint(82, 47);
-		midNeutralToRightHomeZone.addPoint(110, 25);
+		farRightAllianceToRightHomeZoneIndex = purePursuit->addPath(farRightAllianceToRightHomeZone.getPath(0.1));
 
-		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone);
+		QuadraticSplinePath rightHomeZoneToRightAlliance("rightHomeZoneToRightAlliance");
 
-		//!SECTION
-			
-		// SECTION Left AWP
-		
-		QuadraticSplinePath leftHomeZoneToLeftAlliance;
-		
-		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 30), Vector(10.0, M_PI)));
-		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(35, 20), Vector(15.0, M_PI + M_PI_4)));
+		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)));
 
-		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.01));
+		rightHomeZoneToRightAllianceIndex = purePursuit->addPath(rightHomeZoneToRightAlliance.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, 40.0), Vector(10.0, -M_PI_2)));
+		QuadraticSplinePath rightAllianceToRightNeutral("rightAllianceToRightNeutral");
 
-		leftAllianceToRightHomeZoneIndex = purePursuit->addPath(leftAllianceToRightHomeZone.getPath(0.01));
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(145, 35), Vector(10.0, 0)));
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(134, 62), Vector(5.0, M_PI_4)));
 
-		QuadraticSplinePath rightHomeZoneToLeftAlliance;
+		rightAllianceToRightNeutralIndex = purePursuit->addPath(rightAllianceToRightNeutral.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 rightNeutralToPlatform("rightNeutralToPlatform");
 
-		QuadraticSplinePath leftAllianceToPreloads;
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(134, 62), Vector(15.0, 0)));
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(110, 115), Vector(15.0, 0)));
 
-		leftAllianceToPreloads.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI_2)));
-		leftAllianceToPreloads.addPoint(SplinePoint(Point(15, 15), Vector(10.0, M_PI_2)));
+		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.1));
 
-		leftAllianceToPreloadsIndex = purePursuit->addPath(leftAllianceToPreloads.getPath(0.01));
+		QuadraticSplinePath rightPlatformToFarRightGoalDropOff2("rightPlatformToFarRightGoalDropOff2");
 
-		QuadraticSplinePath preloadsToLeftAlliance;
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 115), Vector(15.0, M_PI)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI)));
 
-		preloadsToLeftAlliance.addPoint(SplinePoint(Point(35, 15), Vector(10.0, M_PI_2)));
-		preloadsToLeftAlliance.addPoint(SplinePoint(Point(15, 15), Vector(10.0, M_PI_2)));
+		rightPlatformToFarRightGoalDropOff2Index = purePursuit->addPath(rightPlatformToFarRightGoalDropOff2.getPath(0.1));
 
-		preloadsToLeftAllianceIndex = purePursuit->addPath(preloadsToLeftAlliance.getPath(0.01));
+		QuadraticSplinePath farRightGoalDropOff2ToFarLeftAllianceDropOff("farRightGoalDropOff2ToFarLeftAllianceDropOff");
 
-		// !SECTION
+		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI_2)));
+		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI_2)));
 
-		// SECTION Right AWP
+		farRightGoalDropOff2ToFarLeftAllianceDropOffIndex = purePursuit->addPath(farRightGoalDropOff2ToFarLeftAllianceDropOff.getPath(0.1));
 
-		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
-		
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123, 35), Vector(15, - M_PI_2 - 0.4)));
+		QuadraticSplinePath farLeftAllianceDropOffToFarRightGoalDropOff2("farLeftAllianceDropOffToFarRightGoalDropOff2");
 
-		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.01));
+		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, -M_PI_2)));
+		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, -M_PI_2)));
 
-		QuadraticSplinePath rightPlatformToRightAlliance;
-		
-		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 40), Vector(10.0, -M_PI_2)));
-		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(122, 35), Vector(15, -M_PI_2)));
+		farLeftAllianceDropOffToFarRightGoalDropOff2Index = purePursuit->addPath(farLeftAllianceDropOffToFarRightGoalDropOff2.getPath(0.1));
 
-		rightPlatformToRightAllianceIndex = purePursuit->addPath(rightPlatformToRightAlliance.getPath(0.01));
+		QuadraticSplinePath farLeftAllianceDropOffToPlatform("farLeftAllianceDropOffToPlatform");
 
-		QuadraticSplinePath rightAllianceToRightRings;
+		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, 0)));
+		farLeftAllianceDropOffToPlatform.addPoint(SplinePoint(Point(80, 115), Vector(15.0, 0)));
 
-		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)));
+		farLeftAllianceDropOffToPlatformIndex = purePursuit->addPath(farLeftAllianceDropOffToPlatform.getPath(0.1));
 
-		rightAllianceToRightRingsIndex = purePursuit->addPath(rightAllianceToRightRings.getPath(0.01));
+		QuadraticSplinePath platformToEnterFarLeftHomeZone("platformToEnterFarLeftHomeZone");
 
-		QuadraticSplinePath rightRingsToRightHomeZone;
+		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 115), Vector(15.0, M_PI)));
+		platformToEnterFarLeftHomeZone.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI)));
 
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(20.0, M_PI_2)));
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(120, 25), Vector(15, M_PI)));
+		platformToEnterFarLeftHomeZoneIndex = purePursuit->addPath(platformToEnterFarLeftHomeZone.getPath(0.1));
 
-		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.01));
+		QuadraticSplinePath enterFarLeftHomeZoneToNearRightPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		QuadraticSplinePath rightAllianceToLeftAlliance;
-		
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(122, 30), Vector(10.0, M_PI_4*0.7)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(80, 35), Vector(10.0, M_PI_2)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(38, 35), Vector(30, M_PI_2)));
-		rightAllianceToLeftAlliance.addPoint(SplinePoint(Point(32, 15), Vector(30, -M_PI_2)));
+		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)));
 
-		rightAllianceToLeftAllianceIndex = purePursuit->addPath(rightAllianceToLeftAlliance.getPath(0.01));
+		enterFarLeftHomeZoneToNearRightPlatformIndex = purePursuit->addPath(enterFarLeftHomeZoneToNearRightPlatform.getPath(0.1));
 
 		// !SECTION
 
-		QuadraticSplinePath wiggle;
-
-		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)));
-
-		wiggleIndex = purePursuit->addPath(wiggle.getPath(0.01));
 
 		printf("Array size: %d\n", purePursuit->getPaths().size());
 	}
diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp
index 5c26cb0f..8bd286e0 100644
--- a/include/chassis/tankDrive.hpp
+++ b/include/chassis/tankDrive.hpp
@@ -24,6 +24,11 @@ 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);
diff --git a/pathTest.cpp b/pathTest.cpp
index 84f74fcb..7eb70fd0 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,12 +276,13 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath farLeftAllianceToMidGoal("farLeftAllianceToMidGoal");
+		QuadraticSplinePath rightHomeZoneToLeftAlliance;
 
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(20, 105), Vector(20, -M_PI_2)));
-		farLeftAllianceToMidGoal.addPoint(SplinePoint(Point(72, 70), Vector(20, M_PI_2-M_PI_4)));
+		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(98, 37), Vector(20.0, M_PI_2)));
+		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(46, 37), Vector(20, M_PI_2)));
+		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(35, 17), Vector(20, M_PI + M_PI_2)));
 
-	smoothPath = farLeftAllianceToMidGoal.getPath(0.1);
+	smoothPath = rightHomeZoneToLeftAlliance.getPath(0.1);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index 063f0d29..9d255484 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, 9.0);
+TankDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &midLeftMotor, &midRightMotor, &backLeftMotor, &backRightMotor, &imu, 7.0);
 
 Pronounce::TankPurePursuit purePursuit(&drivetrain, &odometry, new PID(0.6, 0, 2.0), 20);
 
@@ -56,6 +55,7 @@ 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);
 
@@ -76,6 +76,8 @@ bool backButtonStatus = false;
 bool backGrabberManual = false;
 uint32_t lastChange = 0;
 
+bool drivetrainBrakeStatus = false;
+
 // SECTION Auton
 
 void backGrabberChange(bool backButtonStatus2) {
@@ -93,7 +95,7 @@ double goalAngle() {
 
 	int x = clawVision.get_by_sig(0, 1).x_middle_coord;
 
-	double angle = 0.0035 * (double) x + 0.02;
+	double angle = 0.0033908 * (double)x + 0.064622;
 
 	return angle;
 }
@@ -110,7 +112,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);
 	}
 }
 
@@ -156,7 +158,7 @@ void placeOnPlatform() {
 	purePursuit.setEnabled(false);
 	purePursuit.setFollowing(false);
 
-	liftButton.setAutonomousAuthority(1100);
+	liftButton.setAutonomousAuthority(1300);
 
 	pros::Task::delay(300);
 
@@ -242,7 +244,7 @@ void grabFromHook() {
 
 	drivetrain.skidSteerVelocity(-600, 0);
 
-	pros::Task::delay(250);
+	pros::Task::delay(150);
 
 	drivetrain.skidSteerVelocity(0, 0);
 
@@ -260,13 +262,16 @@ void grabFromHook() {
 
 	pros::Task::delay(200);
 
-	drivetrain.skidSteerVelocity(600, 0);
+	drivetrain.skidSteerVelocity(350, 0);
 
-	pros::Task::delay(500);
+	pros::Task::delay(700);
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
+	drivetrain.skidSteerVelocity(0, 0);
+
 	pros::Task::delay(200);
+	liftButton.setAutonomousAuthority(200);
 
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
@@ -285,6 +290,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);
@@ -305,15 +311,18 @@ void deployBackGrabber() {
 }
 
 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(25);
+	purePursuit.setLookahead(30);
+	purePursuit.setMaxAcceleration(oldAccel * 2);
+
+	purePursuit.setUseVoltage(true);
 
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
@@ -326,17 +335,34 @@ void leftSteal() {
 
 	purePursuit.setCurrentPathIndex(leftNeutralStealToLeftHomeZoneIndex);
 
-	purePursuit.setSpeed(60);
+	purePursuit.setSpeed(65);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+	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.setLookahead(10);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
 
 	grabFromHook();
 }
@@ -344,12 +370,16 @@ void leftSteal() {
 void midSteal() {
 	purePursuit.setSpeed(70);
 
+	frontGrabberButton.setButtonStatus(NEUTRAL);
+	frontHookButton.setButtonStatus(POSITIVE);
+
 	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
 
-	purePursuit.setLookahead(25);
+	purePursuit.setLookahead(30);
+	purePursuit.setMaxAcceleration(oldAccel * 2);
 
-	frontGrabberButton.setButtonStatus(NEUTRAL);
-	frontHookButton.setButtonStatus(POSITIVE);
+	purePursuit.setUseVoltage(true);
 
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
@@ -362,20 +392,43 @@ void midSteal() {
 
 	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
 
+	purePursuit.setUseVoltage(false);
+
 	purePursuit.setSpeed(50);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(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);
 	}
 
-	for (int i = 5; i > 3; i--) {
+	purePursuit.setUseVoltage(false);
+
+	for (int i = 5; i > 2; i--) {
 		purePursuit.setSpeed(i * 10);
-		pros::Task::delay(200);
+		pros::Task::delay(300);
 	}
 
 	waitForDone();
 
 	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
 
 	grabFromHook();
 }
@@ -384,30 +437,45 @@ void rightSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
 	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
 
-	purePursuit.setLookahead(20);
+	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);
@@ -417,6 +485,9 @@ void rightSteal() {
 	waitForDone();
 
 	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
 
 	grabFromHook();
 }
@@ -427,27 +498,56 @@ void rightRightSteal() {
 	frontGrabberButton.setButtonStatus(NEUTRAL);
 	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);
 
 	purePursuit.setSpeed(65);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
+	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();
 }
 
@@ -455,33 +555,57 @@ void rightMidSteal() {
 	purePursuit.setSpeed(70);
 
 	frontGrabberButton.setButtonStatus(NEUTRAL);
-	backGrabberChange(false);
 	frontHookButton.setButtonStatus(POSITIVE);
 
 	double oldLookahead = purePursuit.getLookahead();
+	double oldAccel = purePursuit.getMaxAcceleration();
 
-	purePursuit.setLookahead(30);
+	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.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();
 }
@@ -509,11 +633,9 @@ void leftAWP() {
 }
 
 void rightAWP() {
-	turn(-M_PI_2 * 0.85);
-
 	purePursuit.setCurrentPathIndex(enterRightHomeZoneToRightAllianceIndex);
 
-	waitForDone(20, 2500);
+	waitForDone(20, 2000);
 
 	purePursuit.setSpeed(25);
 
@@ -523,6 +645,8 @@ void rightAWP() {
 
 	pros::Task::delay(100);
 
+	turn(-M_PI_4);
+
 	purePursuit.setCurrentPathIndex(rightAllianceToRightRingsIndex);
 
 	liftButton.setAutonomousAuthority(2000);
@@ -533,82 +657,51 @@ void rightAWP() {
 
 	waitForDone(0.1, 3000);
 
+	purePursuit.setSpeed(60);
+
 	purePursuit.setCurrentPathIndex(rightRingsToRightHomeZoneIndex);
 
 	waitForDone();
 }
 
-void rightAWPToLeftAWP() {
-	purePursuit.setSpeed(40);
-
-	turn(-M_PI_2);
-
-	purePursuit.setCurrentPathIndex(rightPlatformToRightAllianceIndex);
-
-	waitForDone(20, 2500);
-
-	purePursuit.setSpeed(40);
-
-	waitForDone(8, 2500);
-
-	backGrabberChange(true);
-
-	waitForDone(0.1, 2500);
-
-	intakeButton.setButtonStatus(POSITIVE);
-	liftButton.setAutonomousAuthority(600);
-
-	purePursuit.setCurrentPathIndex(rightAllianceToLeftAllianceIndex);
+void rightToLeftAWP() {
+	turn(-M_PI_2*0.6);
 
 	purePursuit.setSpeed(50);
 
-	waitForDone(70);
+	purePursuit.setCurrentPathIndex(rightHomeZoneToLeftHomeZoneIndex);
 
-	intakeButton.setButtonStatus(NEUTRAL);
+	waitForDone();
 
-	backGrabberChange(false);
+	turn(-M_PI_4*0.7);
 
-	purePursuit.setSpeed(50);
+	purePursuit.setCurrentPathIndex(rightHomeZoneToLeftAllianceIndex);
 
-	pros::Task::delay(300);
+	waitForDone(8);
 
-	turn(0);
+	purePursuit.setSpeed(20);
 
 	waitForDone();
 
 	backGrabberChange(true);
 
-	pros::Task::delay(200);
+	pros::Task::delay(800);
 
-	intakeButton.setButtonStatus(POSITIVE);
-
-	purePursuit.setSpeed(40);
-
-	for (int i = 0; i < 5; i++) {
-		purePursuit.setCurrentPathIndex(leftAllianceToPreloadsIndex);
-
-		waitForDone();
-
-		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);
 }
 
 /*
@@ -627,30 +720,9 @@ int leftStealLeftAWP() {
 
 	leftSteal();
 
-	purePursuit.setCurrentPathIndex(leftHomeZoneToLeftAllianceIndex);
-
-	waitForDone();
-
-	backGrabberChange(true);
-
-	leftAWP();
-
-	pros::Task::delay(2000);
-
-	backGrabberChange(false);
-
-	return 0;
-}
-
-/**
- * @brief Steal the left side goal and get the full AWP
- *
- * @return int
- */
-int leftStealFullAWP() {
-	odometry.reset(new Position(27, 18, toRadians(10)));
+	purePursuit.setSpeed(30);
 
-	leftSteal();
+	turn(-M_PI_4*0.8);
 
 	purePursuit.setCurrentPathIndex(leftHomeZoneToLeftAllianceIndex);
 
@@ -660,16 +732,8 @@ int leftStealFullAWP() {
 
 	leftAWP();
 
-	waitForDone();
-
-	backGrabberChange(false);
-
 	pros::Task::delay(1000);
 
-	purePursuit.setFollowing(true);
-
-	rightAWP();
-
 	backGrabberChange(false);
 
 	return 0;
@@ -691,26 +755,9 @@ int rightStealRightAWP() {
 
 	rightSteal();
 
-	rightAWP();
+	turn(-M_PI_2 * 1.15);
 
-	backGrabberChange(false);
-
-	pros::Task::delay(200);
-
-	return 0;
-}
-
-/**
- * @brief Steal the right side goal and get the full AWp
- *
- * @return int
- */
-int rightStealFullAWP() {
-	odometry.reset(new Position(105.7, 19.5, 0.0));
-
-	rightSteal();
-
-	rightAWPToLeftAWP();
+	rightAWP();
 
 	backGrabberChange(false);
 
@@ -731,24 +778,13 @@ int rightRightStealToRightAWP() {
 
 	rightRightSteal();
 
-	rightAWP();
+	turn(-M_PI_2 * 0.75);
 
-	return 0;
-}
-
-/**
- * @brief Steal the right side goal starting from the right right side and get the right side AWP
- *
- * @return int
- */
-int rightRightStealToFullAWP() {
-	odometry.reset(new Position(121, 18, toRadians(-15)));
-
-	pros::Task deployBack(deployBackGrabber);
+	rightAWP();
 
-	rightRightSteal();
+	backGrabberChange(false);
 
-	rightAWPToLeftAWP();
+	pros::Task::delay(200);
 
 	return 0;
 }
@@ -759,7 +795,7 @@ int rightRightStealToFullAWP() {
  * @return int
  */
 int midStealToLeftAWP() {
-	odometry.reset(new Position(115, 19.5, 0.0));
+	odometry.reset(new Position(99.5, 18, toRadians(-30)));
 
 	pros::Task deployBack(deployBackGrabber);
 
@@ -771,35 +807,22 @@ int midStealToLeftAWP() {
 }
 
 /**
- * @brief Steal the mid goal from the right side and get the Left AWP
+ * @brief Steal the mid goal from the right side and get the right AWP
  *
  * @return int
  */
 int midStealToRightAWP() {
-	odometry.reset(new Position(99.5, 18, 25.0));
+	odometry.reset(new Position(99.5, 18, toRadians(-30)));
 
 	pros::Task deployBack(deployBackGrabber);
 
 	midSteal();
+	
+	turn(-M_PI_2 * 1.3);
 
 	rightAWP();
 
-	return 0;
-}
-
-/**
- * @brief Steal the mid goal from the right side and get the full AWP
- *
- * @return int
- */
-int midStealToFullAWP() {
-	odometry.reset(new Position(99.5, 18, toRadians(-30)));
-
-	pros::Task deployBack(deployBackGrabber);
-
-	midSteal();
-
-	rightAWPToLeftAWP();
+	backGrabberChange(false);
 
 	return 0;
 }
@@ -864,6 +887,12 @@ int skills() {
 
 	liftButton.setAutonomousAuthority(2000);
 
+	while(odometry.getPosition()->getX() < 55) {
+		pros::Task::delay(50);
+	}
+
+	purePursuit.setSpeed(30);
+
 	waitForDone(0.5, 3000);
 
 	purePursuit.setSpeed(60);
@@ -874,70 +903,32 @@ int skills() {
 
 	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);
-
-	purePursuit.setCurrentPathIndex(farRightDropOffToFarLeftAllianceGoalIndex);
-
-	purePursuit.setSpeed(60);
-
-	waitForDone(30);
-
-	liftButton.setAutonomousAuthority(600);
-
-	purePursuit.setEnabled(false);
-	purePursuit.setFollowing(false);
-
-	pros::Task::delay(500);
-
-	purePursuit.setEnabled(true);
-	purePursuit.setFollowing(true);
-
-	turn(M_PI_2);
-
-	purePursuit.setSpeed(20);
-
-	waitForDone(0.5, 1500);
-
-	backGrabberChange(true);
-
-	pros::Task::delay(100);
-
-	purePursuit.setSpeed(30);
+	liftButton.setAutonomousAuthority(0);
 
-	turn(M_PI * 0.9, 0.3);
+	turn(M_PI);
 
-	purePursuit.setCurrentPathIndex(farLeftAllianceToMidGoalIndex);
+	purePursuit.setCurrentPathIndex(enterFarHomeZoneToMidGoalIndex);
 
 	intakeButton.setButtonStatus(POSITIVE);
 
+	// Slow down on the way to the tall goal
+
 	purePursuit.setSpeed(30);
 
 	waitForDone(30);
 
 	purePursuit.setSpeed(20);
 
-	waitForDone(10);
-
-	liftButton.setAutonomousAuthority(0);
-
 	waitForDone();
 
+	// Grab the middle goal
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
 	pros::Task::delay(300);
@@ -946,6 +937,7 @@ int skills() {
 
 	turn(0);
 
+	// Place the middle goal on the platform
 	purePursuit.setCurrentPathIndex(midGoalToFarPlatformIndex);
 
 	liftButton.setAutonomousAuthority(2000);
@@ -954,44 +946,54 @@ int skills() {
 
 	placeOnPlatform();
 
-	turn(M_PI_2);
+	// Move to drop off the alliance goal
+	purePursuit.setCurrentPathIndex(farPlatformToRightAllianceDropOffIndex);
 
-	purePursuit.setCurrentPathIndex(farPlatformToFarLeftAllianceDropOffIndex);
+	waitForDone();
 
 	liftButton.setAutonomousAuthority(0);
 
-	waitForDone();
+	// Flip goal to front
+	{
+		backGrabberChange(false);
 
-	turn(M_PI_2);
+		purePursuit.setEnabled(false);
+		purePursuit.setFollowing(false);
 
-	intakeButton.setButtonStatus(NEUTRAL);
+		drivetrain.skidSteerVelocity(-300, 0);
 
-	pros::Task::delay(200);
+		pros::Task::delay(200);
 
-	backGrabberChange(false);
+		purePursuit.setEnabled(true);
+		purePursuit.setFollowing(true);
 
-	pros::Task::delay(500);
+		turn(M_PI + 1);
 
-	purePursuit.setCurrentPathIndex(farLeftAllianceDropOffToFarRightDropOffIndex);
+		purePursuit.setEnabled(false);
+		purePursuit.setFollowing(false);
 
-	waitForDone();
+		pros::Task::delay(2000);
 
-	frontGrabberButton.setButtonStatus(POSITIVE);
+		drivetrain.skidSteerVelocity(300, 0);
 
-	pros::Task::delay(200);
+		pros::Task::delay(200);
 
-	liftButton.setAutonomousAuthority(2000);
+		purePursuit.setEnabled(true);
+		purePursuit.setFollowing(true);
 
-	pros::Task::delay(1000);
+		frontGrabberButton.setButtonStatus(POSITIVE);
+	}
 
-	turn(0);
+	liftButton.setAutonomousAuthority(2000);
 
-	pros::Task::delay(200);
+	turn(0);
 
-	purePursuit.setCurrentPathIndex(farRightDropOffToPlatformIndex);
+	// Place near alliance goal on platform
+	purePursuit.setCurrentPathIndex(rightAllianceToFarPlatformIndex);
 
 	placeOnPlatform();
 
+	// Move to the far right alliance goal
 	purePursuit.setCurrentPathIndex(platformToEnterFarRightHomeZoneIndex);
 
 	waitForDone();
@@ -1272,6 +1274,7 @@ int postAuton() {
 	backGrabberButton.setAutonomous(false);
 	liftButton.setAutonomous(false);
 	intakeButton.setAutonomous(false);
+	brakesButton.setAutonomous(false);
 
 	balance.setEnabled(false);
 
@@ -1450,6 +1453,8 @@ void updateMotors() {
 
 		intakeButton.update();
 
+		brakesButton.update();
+
 		pros::Task::delay(20);
 	}
 }
@@ -1478,6 +1483,9 @@ void initMotors() {
 	backGrabberButton2.setSolenoid(&backGrabber);
 	backGrabberButton2.setSingleToggle(true);
 
+	brakesButton.setSolenoid(&brakes);
+	brakesButton.setSingleToggle(true);
+
 	intakeButton.setSingleToggle(true);
 	intakeButton.setDejam(true);
 	intakeButton.setDejamAuthority(-200);
@@ -1497,7 +1505,7 @@ void initDrivetrain() {
 	// 1.072124756
 	// Left 99.57
 	// Right 100.57
-	double turningFactor = (((100.79 / 100.35) - 1.0) / 2);
+	double turningFactor = (((100.35 / 100.0) - 1.0) / 2);
 	double tuningFactor = 0.998791278;
 	leftOdomWheel.setRadius(2.75 / 2);
 	leftOdomWheel.setTuningFactor(tuningFactor * (1 - turningFactor));
@@ -1670,7 +1678,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	midStealToFullAWP();
+	rightRightStealToRightAWP();
 	postAuton();
 
 	// autonomousSelector.run();
@@ -1712,38 +1720,29 @@ 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.skidSteerVoltage(leftY < 0 ? leftY * (12000.0/600.0) : 0, rightX * (12000.0/600.0));
+			}
+			else {
+				drivetrain.skidSteerVoltage(leftY * (12000.0/600.0), rightX * (12000.0/600.0));
+			}
 		}
 		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 (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);
-		}
-		else if (master.get_digital_new_press(DIGITAL_LEFT)) {
-			drivetrain.getLeftMotors().set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
-			drivetrain.getRightMotors().set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
+			if (brakesButton.getButtonStatus() == POSITIVE) {
+				drivetrain.tankSteerVoltage(leftY < 0 ? leftY * (12000.0/600.0) : 0, rightY < 0 ? rightY * (12000.0/600.0) : 0);
+			}
+			else {
+				drivetrain.tankSteerVoltage(leftY * (12000.0/600.0), rightY * (12000.0/600.0));
+			}
 		}
 
-		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_LEFT)) {
+			drivetrainBrakeStatus = !drivetrainBrakeStatus;
+			drivetrain.getLeftMotors().set_brake_mode(drivetrainBrakeStatus ? pros::E_MOTOR_BRAKE_HOLD : pros::E_MOTOR_BRAKE_COAST);
+			drivetrain.getRightMotors().set_brake_mode(drivetrainBrakeStatus ? pros::E_MOTOR_BRAKE_HOLD : pros::E_MOTOR_BRAKE_COAST);
 		}
 
 		// if (master.get_digital_new_press(DIGITAL_X)) {
diff --git a/src/motionControl/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp
index d4caa2fb..10b3c36e 100644
--- a/src/motionControl/tankPurePursuit.cpp
+++ b/src/motionControl/tankPurePursuit.cpp
@@ -90,7 +90,7 @@ namespace Pronounce {
 		double motorSpeed = clamp(clamp(speed, -maxSpeed, maxSpeed), -this->getSpeed(), this->getSpeed()) * this->getOutputMultiplier();
 
 		if (useVoltage) {
-			drivetrain->driveCurvatureVoltage(clamp(motorSpeed * (12000/200.0), -12000.0, 12000.0), pointData.curvature);
+			drivetrain->driveCurvatureVoltage(motorSpeed * (12000/600.0), pointData.curvature);
 		} else {
 			drivetrain->driveCurvature(motorSpeed, pointData.curvature);
 		}

From b46dc8c8f4beeeaf007be78c26c5d61061ce392e Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Sat, 30 Apr 2022 20:45:53 -0600
Subject: [PATCH 09/13] All autons working

---
 include/autoPaths.hpp | 10 +++++-----
 src/main.cpp          |  8 +++++---
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 8ce24e1e..31697cfd 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -168,8 +168,8 @@ namespace Pronounce {
 		Path midNeutralToRightHomeZone;
 
 		midNeutralToRightHomeZone.addPoint(99.5+(sin(toRadians(angle)) * midGoalDistance), 18+(cos(toRadians(angle)) * midGoalDistance));
-		midNeutralToRightHomeZone.addPoint(107, 50);
-		midNeutralToRightHomeZone.addPoint(112, 25);
+		midNeutralToRightHomeZone.addPoint(115, 50);
+		midNeutralToRightHomeZone.addPoint(120, 25);
 
 		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone);
 
@@ -180,7 +180,7 @@ namespace Pronounce {
 		QuadraticSplinePath leftHomeZoneToLeftAlliance;
 		
 		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 30), Vector(10.0, M_PI)));
-		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(40, 15), Vector(20, M_PI + M_PI_4)));
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(37, 20), Vector(20, M_PI + M_PI_4)));
 
 		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.1));
 
@@ -194,8 +194,8 @@ namespace Pronounce {
 
 		QuadraticSplinePath rightHomeZoneToLeftHomeZone;
 
-		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(110, 34), Vector(0.0, M_PI_2)));
-		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(25, 34), Vector(0.0, M_PI_2)));
+		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(110, 32), Vector(0.0, M_PI_2)));
+		rightHomeZoneToLeftHomeZone.addPoint(SplinePoint(Point(25, 30), Vector(0.0, M_PI_2)));
 
 		rightHomeZoneToLeftHomeZoneIndex = purePursuit->addPath(rightHomeZoneToLeftHomeZone.getPath(0.2));
 
diff --git a/src/main.cpp b/src/main.cpp
index 9d255484..bdbbfbdd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -264,7 +264,7 @@ void grabFromHook() {
 
 	drivetrain.skidSteerVelocity(350, 0);
 
-	pros::Task::delay(700);
+	pros::Task::delay(550);
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
@@ -778,7 +778,7 @@ int rightRightStealToRightAWP() {
 
 	rightRightSteal();
 
-	turn(-M_PI_2 * 0.75);
+	turn(-M_PI_2 * 1.4);
 
 	rightAWP();
 
@@ -797,6 +797,8 @@ int rightRightStealToRightAWP() {
 int midStealToLeftAWP() {
 	odometry.reset(new Position(99.5, 18, toRadians(-30)));
 
+	pros::Task::delay(200);
+
 	pros::Task deployBack(deployBackGrabber);
 
 	midSteal();
@@ -1678,7 +1680,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	rightRightStealToRightAWP();
+	midStealToLeftAWP();
 	postAuton();
 
 	// autonomousSelector.run();

From 925c0fcdd2776aab76d9464565e37778c7a11276 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Sun, 1 May 2022 11:18:56 -0600
Subject: [PATCH 10/13] Working on mid and right Autons

---
 include/autoPaths.hpp | 19 ++++++++---------
 pathTest.cpp          | 13 +++++++-----
 src/main.cpp          | 49 ++++++++++++++++++++++++++++++++-----------
 3 files changed, 54 insertions(+), 27 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 31697cfd..4da0f42a 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -168,8 +168,8 @@ namespace Pronounce {
 		Path midNeutralToRightHomeZone;
 
 		midNeutralToRightHomeZone.addPoint(99.5+(sin(toRadians(angle)) * midGoalDistance), 18+(cos(toRadians(angle)) * midGoalDistance));
-		midNeutralToRightHomeZone.addPoint(115, 50);
-		midNeutralToRightHomeZone.addPoint(120, 25);
+		midNeutralToRightHomeZone.addPoint(105, 35);
+		midNeutralToRightHomeZone.addPoint(105, 25);
 
 		midNeutralToRightHomeZoneIndex = purePursuit->addPath(midNeutralToRightHomeZone);
 
@@ -213,30 +213,29 @@ namespace Pronounce {
 		QuadraticSplinePath enterRightHomeZoneToRightAlliance;
 		
 		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
-		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(127, 32), Vector(15, -M_PI_2)));
+		enterRightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(125, 32), Vector(15, -M_PI_2)));
 
 		enterRightHomeZoneToRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToRightAlliance.getPath(0.1));
 
 		QuadraticSplinePath rightPlatformToRightAlliance;
 		
-		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 40), Vector(10.0, -M_PI_2)));
+		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(98, 46.8), Vector(10.0, -M_PI_2)));
 		rightPlatformToRightAlliance.addPoint(SplinePoint(Point(125, 35), Vector(15, -M_PI_2)));
 
 		rightPlatformToRightAllianceIndex = purePursuit->addPath(rightPlatformToRightAlliance.getPath(0.1));
 
 		QuadraticSplinePath rightAllianceToRightRings;
 
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 37), Vector(5, M_PI_4)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 55), Vector(10.0, 0)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 60), Vector(10.0, 0)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(125, 70), Vector(10.0, -M_PI_2)));
+		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)));
 
 		rightAllianceToRightRingsIndex = purePursuit->addPath(rightAllianceToRightRings.getPath(0.1));
 
 		QuadraticSplinePath rightRingsToRightHomeZone;
 
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(20.0, M_PI_2)));
-		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(120, 25), Vector(15, M_PI)));
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(137, 70), Vector(40, M_PI_2*0.9)));
+		rightRingsToRightHomeZone.addPoint(SplinePoint(Point(110, 30), Vector(15, M_PI)));
 
 		rightRingsToRightHomeZoneIndex = purePursuit->addPath(rightRingsToRightHomeZone.getPath(0.1));
 
diff --git a/pathTest.cpp b/pathTest.cpp
index 7eb70fd0..035b9ff1 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,13 +276,16 @@ int main() {
 
 	Path smoothPath;
 
-		QuadraticSplinePath rightHomeZoneToLeftAlliance;
+		double midGoalDistance = 32.66;
+		double angle = -30;
+		
+		QuadraticSplinePath rightAllianceToRightRings;
 
-		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(98, 37), Vector(20.0, M_PI_2)));
-		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(46, 37), Vector(20, M_PI_2)));
-		rightHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(35, 17), Vector(20, M_PI + M_PI_2)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(127, 37), Vector(10, M_PI_4*0.75)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 55), Vector(10.0, 0)));
+		rightAllianceToRightRings.addPoint(SplinePoint(Point(130, 65), Vector(10.0, -M_PI_2)));
 
-	smoothPath = rightHomeZoneToLeftAlliance.getPath(0.1);
+	smoothPath = rightAllianceToRightRings.getPath(0.2);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index bdbbfbdd..a7fa01ad 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -253,6 +253,8 @@ void grabFromHook() {
 	purePursuit.setEnabled(true);
 	purePursuit.setFollowing(true);
 
+	purePursuit.setSpeed(60);
+
 	turn(odometry.getPosition()->getTheta() + angle);
 
 	pros::Task::delay(200);
@@ -268,9 +270,11 @@ void grabFromHook() {
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
+	pros::Task::delay(50);
+
 	drivetrain.skidSteerVelocity(0, 0);
 
-	pros::Task::delay(200);
+	pros::Task::delay(400);
 	liftButton.setAutonomousAuthority(200);
 
 	purePursuit.setEnabled(true);
@@ -376,8 +380,8 @@ void midSteal() {
 	double oldLookahead = purePursuit.getLookahead();
 	double oldAccel = purePursuit.getMaxAcceleration();
 
-	purePursuit.setLookahead(30);
-	purePursuit.setMaxAcceleration(oldAccel * 2);
+	purePursuit.setLookahead(20);
+	purePursuit.setMaxAcceleration(oldAccel * 1.5);
 
 	purePursuit.setUseVoltage(true);
 
@@ -390,6 +394,9 @@ void midSteal() {
 
 	frontHookButton.setButtonStatus(NEUTRAL);
 
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+	
 	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
 
 	purePursuit.setUseVoltage(false);
@@ -576,6 +583,8 @@ void rightMidSteal() {
 
 	waitForDone();
 
+	purePursuit.setLookahead(oldLookahead);
+
 	purePursuit.setCurrentPathIndex(midNeutralToRightHomeZoneIndex);
 
 	purePursuit.setSpeed(65);
@@ -637,15 +646,21 @@ void rightAWP() {
 
 	waitForDone(20, 2000);
 
-	purePursuit.setSpeed(25);
+	purePursuit.setSpeed(40);
 
-	waitForDone(0.1, 1000);
+	liftButton.setAutonomousAuthority(600);
+
+	waitForDone(0.1, 2000);
 
 	backGrabberChange(true);
 
 	pros::Task::delay(100);
 
-	turn(-M_PI_4);
+	purePursuit.setSpeed(60);
+
+	turn(-M_PI_4*0.5);
+
+	purePursuit.setSpeed(25);
 
 	purePursuit.setCurrentPathIndex(rightAllianceToRightRingsIndex);
 
@@ -661,10 +676,17 @@ void rightAWP() {
 
 	purePursuit.setCurrentPathIndex(rightRingsToRightHomeZoneIndex);
 
+	waitForDone(5);
+
+	backGrabberChange(false);
+
 	waitForDone();
 }
 
 void rightToLeftAWP() {
+
+	purePursuit.setSpeed(65);
+
 	turn(-M_PI_2*0.6);
 
 	purePursuit.setSpeed(50);
@@ -720,10 +742,12 @@ int leftStealLeftAWP() {
 
 	leftSteal();
 
-	purePursuit.setSpeed(30);
+	purePursuit.setSpeed(60);
 
 	turn(-M_PI_4*0.8);
 
+	purePursuit.setSpeed(30);
+
 	purePursuit.setCurrentPathIndex(leftHomeZoneToLeftAllianceIndex);
 
 	waitForDone();
@@ -754,8 +778,9 @@ int rightStealRightAWP() {
 	pros::Task deployBack(deployBackGrabber);
 
 	rightSteal();
-
-	turn(-M_PI_2 * 1.15);
+	
+	purePursuit.setSpeed(40);
+	turn(-M_PI_2 * 1.07);
 
 	rightAWP();
 
@@ -820,7 +845,7 @@ int midStealToRightAWP() {
 
 	midSteal();
 	
-	turn(-M_PI_2 * 1.3);
+	turn(-M_PI_2 * 1.15);
 
 	rightAWP();
 
@@ -1437,7 +1462,7 @@ void updateMotors() {
 
 		if (backButtonStatus && !backGrabberManual) {
 			backGrabberButton2.setButtonStatus(POSITIVE);
-			if (pros::millis() - lastChange > 100) {
+			if (pros::millis() - lastChange > 150) {
 				backGrabberButton.setButtonStatus(POSITIVE);
 			}
 		}
@@ -1680,7 +1705,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	midStealToLeftAWP();
+	rightStealRightAWP();
 	postAuton();
 
 	// autonomousSelector.run();

From 13ea3a6f9754e98445365ef6bc6afaf7a3d7c189 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Sun, 1 May 2022 17:44:20 -0600
Subject: [PATCH 11/13] Add double steal

---
 include/autoPaths.hpp |  57 ++++++++++++---------
 src/main.cpp          | 115 ++++++++++++++++++++++++++++++++++++++----
 2 files changed, 139 insertions(+), 33 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index 4da0f42a..e48de906 100644
--- a/include/autoPaths.hpp
+++ b/include/autoPaths.hpp
@@ -50,6 +50,7 @@ namespace Pronounce {
 	// Right steal
 	int rightHomeZoneToRightNeutralIndex = -1;
 	int rightNeutralToRightHomeZoneIndex = -1;
+	int rightHomeZoneToRightNeutralClawIndex = -1;
 
 	// Right(Right position) steal
 	int farRightHomeZoneToRightNeutralIndex = -1;
@@ -130,11 +131,18 @@ namespace Pronounce {
 
 		QuadraticSplinePath rightNeutralToRightHomeZone;
 
-		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 43), Vector(0.0, 0)));
+		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 55), Vector(0.0, 0)));
 		rightNeutralToRightHomeZone.addPoint(SplinePoint(Point(107, 17), Vector(0.0, 0)));
 
 		rightNeutralToRightHomeZoneIndex = purePursuit->addPath(rightNeutralToRightHomeZone.getPath(0.1));
 
+		QuadraticSplinePath rightHomeZoneToRightNeutralClaw;
+
+		rightHomeZoneToRightNeutralClaw.addPoint(SplinePoint(Point(107, 19.5), Vector(0.0, 0)));
+		rightHomeZoneToRightNeutralClaw.addPoint(SplinePoint(Point(107, 55), Vector(0.0, 0)));
+
+		rightHomeZoneToRightNeutralClawIndex = purePursuit->addPath(rightHomeZoneToRightNeutralClaw.getPath(0.1));
+
 		// !SECTION
 
 		// SECTION Right right steal
@@ -180,7 +188,7 @@ namespace Pronounce {
 		QuadraticSplinePath leftHomeZoneToLeftAlliance;
 		
 		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(25, 30), Vector(10.0, M_PI)));
-		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(37, 20), Vector(20, M_PI + M_PI_4)));
+		leftHomeZoneToLeftAlliance.addPoint(SplinePoint(Point(37, 17), Vector(20, M_PI + M_PI_4)));
 
 		leftHomeZoneToLeftAllianceIndex = purePursuit->addPath(leftHomeZoneToLeftAlliance.getPath(0.1));
 
@@ -247,28 +255,28 @@ namespace Pronounce {
 		// Left home zone to left neutral goal
 		QuadraticSplinePath leftHomeZoneToLeftNeutralGoal("Left Home Zone to Left Neutral Goal");
 
-		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(26.5, 11), Vector(20, M_PI_2)));
-		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(38, 67), Vector(10, 0)));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(26.5, 11), Vector(15, M_PI_2)));
+		leftHomeZoneToLeftNeutralGoal.addPoint(SplinePoint(Point(35, 67), Vector(10, 0)));
 
 		leftHomeZoneToLeftNeutralGoalIndex = purePursuit->addPath(leftHomeZoneToLeftNeutralGoal.getPath(0.1));
 
 		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
 
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 72), Vector(25, -M_PI_4)));
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(78, 110), Vector(25, 0.1)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 67), Vector(15, -M_PI_4)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(68, 110), Vector(25, 0.1)));
 
 		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
 
 		QuadraticSplinePath farPlatformToEnterFarHomeZone("Left Home Zone to far right goal drop off");
 
-		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(78, 107), Vector(0.0, 0.0)));
-		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(78, 95), Vector(0.0, 0.0)));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(73, 107), Vector(0.0, 0.0)));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(73, 95), Vector(0.0, 0.0)));
 
 		farPlatformToEnterFarHomeZoneIndex = purePursuit->addPath(farPlatformToEnterFarHomeZone.getPath(0.1));
 
 		QuadraticSplinePath enterFarHomeZoneToMidGoal("farLeftAllianceToMidGoal");
 
-		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(78, 95), Vector(5.0, -M_PI)));
+		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(73, 95), Vector(5.0, -M_PI)));
 		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(70, 70), Vector(10, -M_PI)));
 
 		enterFarHomeZoneToMidGoalIndex = purePursuit->addPath(enterFarHomeZoneToMidGoal.getPath(0.2));
@@ -276,35 +284,36 @@ namespace Pronounce {
 		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
 
 		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(85, 107), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 110), Vector(0.0, 0)));
 
 		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.2));
 
 		QuadraticSplinePath farPlatformToRightAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
 
 		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 107), Vector(0.0, 0)));
-		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 90), Vector(0.0, 0)));
+		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(70, 80), Vector(0.0, 0)));
 
 		farPlatformToRightAllianceDropOffIndex = purePursuit->addPath(farPlatformToRightAllianceDropOff.getPath(0.1));
 		
 		QuadraticSplinePath rightAllianceToFarPlatform("farRightDropOffToPlatform");
 
-		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(80, 90), Vector(7.0, 0)));
-		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(85, 107), Vector(7.0, 0)));
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(70, 80), Vector(7.0, 0)));
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(70, 107), Vector(7.0, 0)));
 
 		rightAllianceToFarPlatformIndex = purePursuit->addPath(rightAllianceToFarPlatform.getPath(0.1));
 
 		QuadraticSplinePath platformToEnterFarRightHomeZone("platformToEnterFarRightHomeZone");
 
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 115), Vector(7.0, M_PI)));
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(85, 105.7), Vector(7.0, M_PI)));
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(70, 115), Vector(7.0, M_PI)));
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(70, 100), Vector(7.0, M_PI)));
 
 		platformToEnterFarRightHomeZoneIndex = purePursuit->addPath(platformToEnterFarRightHomeZone.getPath(0.1));
 
 		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
 
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 105.7), Vector(20.0, -M_PI_2)));
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(105, 122), Vector(35, M_PI_2)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(70, 100), Vector(20.0, -M_PI_2)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 100), Vector(20.0, -M_PI_2)));
+		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(114, 122), Vector(35, M_PI_2*1.1)));
 
 		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.1));
 
@@ -326,34 +335,34 @@ namespace Pronounce {
 		QuadraticSplinePath rightAllianceToRightNeutral("rightAllianceToRightNeutral");
 
 		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(145, 35), Vector(10.0, 0)));
-		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(134, 62), Vector(5.0, M_PI_4)));
+		rightAllianceToRightNeutral.addPoint(SplinePoint(Point(122, 62), Vector(5.0, M_PI_4)));
 
 		rightAllianceToRightNeutralIndex = purePursuit->addPath(rightAllianceToRightNeutral.getPath(0.1));
 
 		QuadraticSplinePath rightNeutralToPlatform("rightNeutralToPlatform");
 
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(134, 62), Vector(15.0, 0)));
-		rightNeutralToPlatform.addPoint(SplinePoint(Point(110, 115), Vector(15.0, 0)));
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(122, 62), Vector(15.0, 0)));
+		rightNeutralToPlatform.addPoint(SplinePoint(Point(100, 115), Vector(15.0, 0)));
 
 		rightNeutralToPlatformIndex = purePursuit->addPath(rightNeutralToPlatform.getPath(0.1));
 
 		QuadraticSplinePath rightPlatformToFarRightGoalDropOff2("rightPlatformToFarRightGoalDropOff2");
 
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 115), Vector(15.0, M_PI)));
-		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(100, 115), Vector(15.0, M_PI)));
+		rightPlatformToFarRightGoalDropOff2.addPoint(SplinePoint(Point(110, 100), Vector(15.0, M_PI)));
 
 		rightPlatformToFarRightGoalDropOff2Index = purePursuit->addPath(rightPlatformToFarRightGoalDropOff2.getPath(0.1));
 
 		QuadraticSplinePath farRightGoalDropOff2ToFarLeftAllianceDropOff("farRightGoalDropOff2ToFarLeftAllianceDropOff");
 
 		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(110, 105.7), Vector(15.0, M_PI_2)));
-		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, M_PI_2)));
+		farRightGoalDropOff2ToFarLeftAllianceDropOff.addPoint(SplinePoint(Point(25, 105.7), Vector(15.0, M_PI_2)));
 
 		farRightGoalDropOff2ToFarLeftAllianceDropOffIndex = purePursuit->addPath(farRightGoalDropOff2ToFarLeftAllianceDropOff.getPath(0.1));
 
 		QuadraticSplinePath farLeftAllianceDropOffToFarRightGoalDropOff2("farLeftAllianceDropOffToFarRightGoalDropOff2");
 
-		farLeftAllianceDropOffToFarRightGoalDropOff2.addPoint(SplinePoint(Point(80, 105.7), Vector(15.0, -M_PI_2)));
+		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)));
 
 		farLeftAllianceDropOffToFarRightGoalDropOff2Index = purePursuit->addPath(farLeftAllianceDropOffToFarRightGoalDropOff2.getPath(0.1));
diff --git a/src/main.cpp b/src/main.cpp
index a7fa01ad..8f5ad234 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -127,7 +127,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);
 	}
 
@@ -744,7 +744,7 @@ int leftStealLeftAWP() {
 
 	purePursuit.setSpeed(60);
 
-	turn(-M_PI_4*0.8);
+	turn(-M_PI_4*0.5);
 
 	purePursuit.setSpeed(30);
 
@@ -854,6 +854,97 @@ int midStealToRightAWP() {
 	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);
+
+	purePursuit.setLookahead(oldLookahead);
+	purePursuit.setMaxAcceleration(oldAccel);
+
+	brakesButton.setButtonStatus(NEUTRAL);
+
+	turn(0);
+
+	purePursuit.setSpeed(60);
+
+	purePursuit.setCurrentPathIndex(rightHomeZoneToRightNeutralClawIndex);
+
+	waitForDone();
+
+	frontGrabberButton.setButtonStatus(POSITIVE);
+
+	pros::Task::delay(100);
+
+	purePursuit.setCurrentPathIndex(rightHomeZoneToRightNeutralClawIndex);
+
+	waitForDone();
+
+	return 0;
+}
+
 int skills() {
 	odometry.reset(new Position(26.5, 11, -M_PI_2));
 
@@ -935,10 +1026,10 @@ int skills() {
 
 	waitForDone();
 
-	purePursuit.setSpeed(60);
-
 	liftButton.setAutonomousAuthority(0);
 
+	purePursuit.setSpeed(30);
+
 	turn(M_PI);
 
 	purePursuit.setCurrentPathIndex(enterFarHomeZoneToMidGoalIndex);
@@ -960,6 +1051,10 @@ int skills() {
 
 	pros::Task::delay(300);
 
+	liftButton.setAutonomousAuthority(600);
+
+	pros::Task::delay(500);
+
 	purePursuit.setSpeed(60);
 
 	turn(0);
@@ -984,17 +1079,19 @@ int skills() {
 	{
 		backGrabberChange(false);
 
+		pros::Task::delay(400);
+
 		purePursuit.setEnabled(false);
 		purePursuit.setFollowing(false);
 
-		drivetrain.skidSteerVelocity(-300, 0);
+		drivetrain.skidSteerVelocity(300, 0);
 
-		pros::Task::delay(200);
+		pros::Task::delay(300);
 
 		purePursuit.setEnabled(true);
 		purePursuit.setFollowing(true);
 
-		turn(M_PI + 1);
+		turn(odometry.getPosition()->getTheta() + M_PI);
 
 		purePursuit.setEnabled(false);
 		purePursuit.setFollowing(false);
@@ -1003,7 +1100,7 @@ int skills() {
 
 		drivetrain.skidSteerVelocity(300, 0);
 
-		pros::Task::delay(200);
+		pros::Task::delay(400);
 
 		purePursuit.setEnabled(true);
 		purePursuit.setFollowing(true);
@@ -1705,7 +1802,7 @@ void autonomous() {
 	// autonRoutines.hpp and the implementation is autonRoutines.cpp
 	// autonomousSelector.run();
 	preAutonRun();
-	rightStealRightAWP();
+	skills();
 	postAuton();
 
 	// autonomousSelector.run();

From 25c08572779904aa685c32ce61227da311bf7987 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Mon, 2 May 2022 21:07:33 -0600
Subject: [PATCH 12/13] Skills

---
 include/autoPaths.hpp |  63 ++++++++++++-----
 pathTest.cpp          |  14 ++--
 src/main.cpp          | 157 +++++++++---------------------------------
 3 files changed, 81 insertions(+), 153 deletions(-)

diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
index e48de906..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,11 +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;
@@ -263,20 +265,20 @@ namespace Pronounce {
 		QuadraticSplinePath leftNeutralGoalToFarHomeZone("Left Neutral Goal to Far Home Zone");
 
 		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(35, 67), Vector(15, -M_PI_4)));
-		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(68, 110), Vector(25, 0.1)));
+		leftNeutralGoalToFarHomeZone.addPoint(SplinePoint(Point(70, 112), Vector(25, 0.1)));
 
 		leftNeutralGoalToFarHomeZoneIndex = purePursuit->addPath(leftNeutralGoalToFarHomeZone.getPath(0.1));
 
 		QuadraticSplinePath farPlatformToEnterFarHomeZone("Left Home Zone to far right goal drop off");
 
-		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(73, 107), Vector(0.0, 0.0)));
-		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(73, 95), Vector(0.0, 0.0)));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(70, 112), Vector(0.0, 0.0)));
+		farPlatformToEnterFarHomeZone.addPoint(SplinePoint(Point(70, 95), Vector(0.0, 0.0)));
 
 		farPlatformToEnterFarHomeZoneIndex = purePursuit->addPath(farPlatformToEnterFarHomeZone.getPath(0.1));
 
 		QuadraticSplinePath enterFarHomeZoneToMidGoal("farLeftAllianceToMidGoal");
 
-		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(73, 95), Vector(5.0, -M_PI)));
+		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(70, 95), Vector(5.0, -M_PI*0.8)));
 		enterFarHomeZoneToMidGoal.addPoint(SplinePoint(Point(70, 70), Vector(10, -M_PI)));
 
 		enterFarHomeZoneToMidGoalIndex = purePursuit->addPath(enterFarHomeZoneToMidGoal.getPath(0.2));
@@ -284,51 +286,51 @@ namespace Pronounce {
 		QuadraticSplinePath midGoalToFarPlatform("midGoalToFarPlatform");
 
 		midGoalToFarPlatform.addPoint(SplinePoint(Point(70, 70), Vector(0.0, 0)));
-		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 110), Vector(0.0, 0)));
+		midGoalToFarPlatform.addPoint(SplinePoint(Point(80, 115), Vector(0.0, 0)));
 
 		midGoalToFarPlatformIndex = purePursuit->addPath(midGoalToFarPlatform.getPath(0.2));
 
 		QuadraticSplinePath farPlatformToRightAllianceDropOff("farPlatformToFarLeftAllianceDropOff");
 
 		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 107), Vector(0.0, 0)));
-		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(70, 80), Vector(0.0, 0)));
+		farPlatformToRightAllianceDropOff.addPoint(SplinePoint(Point(80, 80), Vector(0.0, 0)));
 
 		farPlatformToRightAllianceDropOffIndex = purePursuit->addPath(farPlatformToRightAllianceDropOff.getPath(0.1));
 		
 		QuadraticSplinePath rightAllianceToFarPlatform("farRightDropOffToPlatform");
 
-		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(70, 80), Vector(7.0, 0)));
-		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(70, 107), Vector(7.0, 0)));
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(80, 80), Vector(7.0, 0)));
+		rightAllianceToFarPlatform.addPoint(SplinePoint(Point(77, 123), Vector(7.0, 0)));
 
 		rightAllianceToFarPlatformIndex = purePursuit->addPath(rightAllianceToFarPlatform.getPath(0.1));
 
 		QuadraticSplinePath platformToEnterFarRightHomeZone("platformToEnterFarRightHomeZone");
 
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(70, 115), Vector(7.0, M_PI)));
-		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(70, 100), Vector(7.0, M_PI)));
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(80, 115), Vector(7.0, M_PI)));
+		platformToEnterFarRightHomeZone.addPoint(SplinePoint(Point(80, 110), Vector(7.0, M_PI)));
 
 		platformToEnterFarRightHomeZoneIndex = purePursuit->addPath(platformToEnterFarRightHomeZone.getPath(0.1));
 
 		QuadraticSplinePath enterRightHomeZoneToFarRightAlliance("enterRightHomeZoneToFarRightAlliance");
 
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(70, 100), Vector(20.0, -M_PI_2)));
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(85, 100), Vector(20.0, -M_PI_2)));
-		enterRightHomeZoneToFarRightAlliance.addPoint(SplinePoint(Point(114, 122), Vector(35, M_PI_2*1.1)));
+		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)));
 
 		enterRightHomeZoneToFarRightAllianceIndex = purePursuit->addPath(enterRightHomeZoneToFarRightAlliance.getPath(0.1));
 
 		QuadraticSplinePath farRightAllianceToRightHomeZone("farRightAllianceToRightHomeZone");
 
-		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, 20), Vector(20.0, M_PI)));
+		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)));
 
 		farRightAllianceToRightHomeZoneIndex = purePursuit->addPath(farRightAllianceToRightHomeZone.getPath(0.1));
 
 		QuadraticSplinePath rightHomeZoneToRightAlliance("rightHomeZoneToRightAlliance");
 
 		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)));
+		rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(135, 35), Vector(10.0, -M_PI_4 * 0.6)));
 
 		rightHomeZoneToRightAllianceIndex = purePursuit->addPath(rightHomeZoneToRightAlliance.getPath(0.1));
 
@@ -389,6 +391,29 @@ namespace Pronounce {
 
 		enterFarLeftHomeZoneToNearRightPlatformIndex = purePursuit->addPath(enterFarLeftHomeZoneToNearRightPlatform.getPath(0.1));
 
+		QuadraticSplinePath farPlatformToFarLeftAllianceGoal("enterFarLeftHomeZoneToNearRightPlatform");
+
+		farPlatformToFarLeftAllianceGoal.addPoint(SplinePoint(Point(80, 110), Vector(25, M_PI_2)));
+		farPlatformToFarLeftAllianceGoal.addPoint(SplinePoint(Point(28, 127), Vector(25, M_PI_2)));
+
+		farPlatformToFarLeftAllianceGoalIndex = purePursuit->addPath(farPlatformToFarLeftAllianceGoal.getPath(0.1));
+
+		QuadraticSplinePath farLeftAllianceGoalToRightNeutralGoal("enterFarLeftHomeZoneToNearRightPlatform");
+
+		farLeftAllianceGoalToRightNeutralGoal.addPoint(SplinePoint(Point(32, 133), Vector(25, -M_PI_2)));
+		farLeftAllianceGoalToRightNeutralGoal.addPoint(SplinePoint(Point(105.7, 70), Vector(40, M_PI)));
+
+		farLeftAllianceGoalToRightNeutralGoalIndex = purePursuit->addPath(farLeftAllianceGoalToRightNeutralGoal.getPath(0.1));
+
+		QuadraticSplinePath rightNeutralGoalToNearRIghtPlatform("enterFarLeftHomeZoneToNearRightPlatform");
+
+		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)));
+
+		rightNeutralGoalToNearRIghtPlatformIndex = purePursuit->addPath(rightNeutralGoalToNearRIghtPlatform.getPath(0.1));
+
 		// !SECTION
 
 
diff --git a/pathTest.cpp b/pathTest.cpp
index 035b9ff1..7a82995e 100644
--- a/pathTest.cpp
+++ b/pathTest.cpp
@@ -276,16 +276,14 @@ int main() {
 
 	Path smoothPath;
 
-		double midGoalDistance = 32.66;
-		double angle = -30;
-		
-		QuadraticSplinePath rightAllianceToRightRings;
+		QuadraticSplinePath rightNeutralGoalToNearRIghtPlatform("enterFarLeftHomeZoneToNearRightPlatform");
 
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(127, 37), Vector(10, M_PI_4*0.75)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(120, 55), Vector(10.0, 0)));
-		rightAllianceToRightRings.addPoint(SplinePoint(Point(130, 65), Vector(10.0, -M_PI_2)));
+		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 = rightAllianceToRightRings.getPath(0.2);
+	smoothPath = rightNeutralGoalToNearRIghtPlatform.getPath(0.1);
 
 	paths.emplace_back(smoothPath);
 
diff --git a/src/main.cpp b/src/main.cpp
index 8f5ad234..ad9528d0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -42,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, 7.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);
 
@@ -196,7 +196,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);
@@ -1030,7 +1034,7 @@ int skills() {
 
 	purePursuit.setSpeed(30);
 
-	turn(M_PI);
+	turn(M_PI*0.8);
 
 	purePursuit.setCurrentPathIndex(enterFarHomeZoneToMidGoalIndex);
 
@@ -1059,6 +1063,8 @@ int skills() {
 
 	turn(0);
 
+	turn(0);
+
 	// Place the middle goal on the platform
 	purePursuit.setCurrentPathIndex(midGoalToFarPlatformIndex);
 
@@ -1086,7 +1092,7 @@ int skills() {
 
 		drivetrain.skidSteerVelocity(300, 0);
 
-		pros::Task::delay(300);
+		pros::Task::delay(150);
 
 		purePursuit.setEnabled(true);
 		purePursuit.setFollowing(true);
@@ -1115,6 +1121,10 @@ int skills() {
 	// Place near alliance goal on platform
 	purePursuit.setCurrentPathIndex(rightAllianceToFarPlatformIndex);
 
+	waitForDone();
+
+	turn(0);
+
 	placeOnPlatform();
 
 	// Move to the far right alliance goal
@@ -1122,67 +1132,31 @@ int skills() {
 
 	waitForDone();
 
-	turn(-M_PI_2);
-
-	purePursuit.setCurrentPathIndex(enterRightHomeZoneToFarRightAllianceIndex);
-
-	waitForDone();
-
-	backGrabberChange(true);
-
-	pros::Task::delay(200);
-
-	purePursuit.setCurrentPathIndex(farRightAllianceToRightHomeZoneIndex);
-
-	pros::Task::delay(200);
-
-	intakeButton.setButtonStatus(POSITIVE);
-
-	liftButton.setAutonomousAuthority(600);
-
-	purePursuit.setSpeed(60);
-
-	while (odometry.getPosition()->getY() > 85) {
-		pros::Task::delay(50);
-	}
+	turn(M_PI_2);
 
 	purePursuit.setSpeed(40);
 
-	while (odometry.getPosition()->getY() > 40) {
-		pros::Task::delay(50);
-	}
+	purePursuit.setCurrentPathIndex(farPlatformToFarLeftAllianceGoalIndex);
 
-	purePursuit.setSpeed(10);
+	waitForDone(20, 4000);
 
-	while (odometry.getPosition()->getY() > 30) {
-		pros::Task::delay(50);
-	}
+	purePursuit.setSpeed(20);
 
-	backGrabberChange(false);
+	waitForDone(0.5, 4000);
 
-	intakeButton.setButtonStatus(NEUTRAL);
+	backGrabberChange(true);
 
 	pros::Task::delay(500);
 
-	purePursuit.setSpeed(60);
-
-	waitForDone();
-
-	turn(-M_PI_2 - M_PI_4);
-
-	purePursuit.setCurrentPathIndex(rightHomeZoneToRightAllianceIndex);
-
-	waitForDone();
-
-	backGrabberChange(true);
+	purePursuit.setSpeed(40);
 
-	pros::Task::delay(100);
+	purePursuit.setCurrentPathIndex(farLeftAllianceGoalToRightNeutralGoalIndex);
 
-	turn(0);
+	liftButton.setAutonomousAuthority(600);
 
-	purePursuit.setCurrentPathIndex(rightAllianceToRightNeutralIndex);
+	waitForDone(40);
 
-	waitForDone(10);
+	purePursuit.setSpeed(20);
 
 	liftButton.setAutonomousAuthority(0);
 
@@ -1190,96 +1164,27 @@ int skills() {
 
 	frontGrabberButton.setButtonStatus(POSITIVE);
 
-	purePursuit.setCurrentPathIndex(rightNeutralToPlatformIndex);
-
-	pros::Task::delay(500);
+	pros::Task::delay(150);
 
 	liftButton.setAutonomousAuthority(2000);
 
-	placeOnPlatform();
-
-	purePursuit.setCurrentPathIndex(rightPlatformToFarRightGoalDropOff2Index);
+	turn(-M_PI_2-M_PI_4);
 
-	liftButton.setAutonomousAuthority(0);
-
-	waitForDone();
-
-	turn(-M_PI_2);
-
-	backGrabberChange(false);
-	intakeButton.setButtonStatus(NEUTRAL);
-
-	purePursuit.setCurrentPathIndex(farRightGoalDropOff2ToFarLeftAllianceDropOffIndex);
-
-	waitForDone(20);
-
-	turn(M_PI_2);
+	purePursuit.setSpeed(40);
 
-	purePursuit.setCurrentPathIndex(farLeftAllianceDropOffToFarRightGoalDropOff2Index);
+	purePursuit.setCurrentPathIndex(rightNeutralGoalToNearRIghtPlatformIndex);
 
 	waitForDone();
 
-	frontGrabberButton.setButtonStatus(POSITIVE);
-
-	pros::Task::delay(200);
-
-	purePursuit.setCurrentPathIndex(farRightGoalDropOff2ToFarLeftAllianceDropOffIndex);
-
 	pros::Task::delay(500);
 
-	liftButton.setAutonomousAuthority(2000);
-
-	waitForDone();
-
-	backGrabberChange(true);
-
-	pros::Task::delay(100);
-
-	intakeButton.setButtonStatus(POSITIVE);
-
-	turn(0);
-
-	purePursuit.setCurrentPathIndex(farLeftAllianceDropOffToPlatformIndex);
-
-	placeOnPlatform();
-
 	turn(M_PI_2);
 
-	purePursuit.setCurrentPathIndex(enterFarLeftHomeZoneToNearRightPlatformIndex);
-
-	purePursuit.setSpeed(60);
-
-	liftButton.setAutonomousAuthority(600);
-
-	while (odometry.getPosition()->getY() > 45) {
-		pros::Task::delay(50);
-	}
-
-	purePursuit.setSpeed(20);
-
-	liftButton.setAutonomousAuthority(0);
-
-	while (odometry.getPosition()->getY() > 35) {
-		pros::Task::delay(50);
-	}
-
-	frontGrabberButton.setButtonStatus(POSITIVE);
-
-	pros::Task::delay(100);
-
-	liftButton.setAutonomousAuthority(2000);
-
-	purePursuit.setSpeed(60);
-
-	waitForDone();
-
-	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);
 

From 5d30d5e03c37c538a6f7451afd5a0a41e42ade15 Mon Sep 17 00:00:00 2001
From: ad101-lab <55064247+ad101-lab@users.noreply.github.com>
Date: Mon, 2 May 2022 21:12:57 -0600
Subject: [PATCH 13/13] Revert brakes and voltage control

---
 src/main.cpp | 21 +++++++++++----------
 1 file changed, 11 insertions(+), 10 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index ad9528d0..5c0b4eaf 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -76,8 +76,6 @@ bool backButtonStatus = false;
 bool backGrabberManual = false;
 uint32_t lastChange = 0;
 
-bool drivetrainBrakeStatus = false;
-
 // SECTION Auton
 
 void backGrabberChange(bool backButtonStatus2) {
@@ -1750,10 +1748,10 @@ void opcontrol() {
 			int rightX = filterAxis(master, ANALOG_RIGHT_X);
 
 			if (brakesButton.getButtonStatus() == POSITIVE) {
-				drivetrain.skidSteerVoltage(leftY < 0 ? leftY * (12000.0/600.0) : 0, rightX * (12000.0/600.0));
+				drivetrain.skidSteerVelocity(leftY < 0 ? leftY : 0, rightX);
 			}
 			else {
-				drivetrain.skidSteerVoltage(leftY * (12000.0/600.0), rightX * (12000.0/600.0));
+				drivetrain.skidSteerVelocity(leftY, rightX);
 			}
 		}
 		else if (!balance.isEnabled()) {
@@ -1761,17 +1759,20 @@ void opcontrol() {
 			int rightY = filterAxis(master, ANALOG_RIGHT_Y);
 
 			if (brakesButton.getButtonStatus() == POSITIVE) {
-				drivetrain.tankSteerVoltage(leftY < 0 ? leftY * (12000.0/600.0) : 0, rightY < 0 ? rightY * (12000.0/600.0) : 0);
+				drivetrain.tankSteerVelocity(leftY < 0 ? leftY : 0, rightY < 0 ? rightY : 0);
 			}
 			else {
-				drivetrain.tankSteerVoltage(leftY * (12000.0/600.0), rightY * (12000.0/600.0));
+				drivetrain.tankSteerVelocity(leftY, rightY);
 			}
 		}
 
-		if (master.get_digital_new_press(DIGITAL_LEFT)) {
-			drivetrainBrakeStatus = !drivetrainBrakeStatus;
-			drivetrain.getLeftMotors().set_brake_mode(drivetrainBrakeStatus ? pros::E_MOTOR_BRAKE_HOLD : pros::E_MOTOR_BRAKE_COAST);
-			drivetrain.getRightMotors().set_brake_mode(drivetrainBrakeStatus ? pros::E_MOTOR_BRAKE_HOLD : pros::E_MOTOR_BRAKE_COAST);
+		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);
+		}
+		else if (master.get_digital_new_press(DIGITAL_LEFT)) {
+			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)) {