Merge branch 'ad-tank-pure-pursuit' of https://github.com/ad101-lab/p…

…ronounce-this into ad-tank-pure-pursuit
alexDickhans · Jan 3, 2022 · 9a370fe · 9a370fe
2 parents 362dde8 + 8bcb894
commit 9a370fe
Show file tree

Hide file tree

Showing 3 changed files with 178 additions and 146 deletions.
diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp
@@ -0,0 +1,157 @@
+#pragma once
+
+#include "utils/pointUtil.hpp"
+#include "utils/path.hpp"
+#include "utils/splinePath.hpp"
+#include "motionControl/purePursuit.hpp"
+#include "utils/purePursuitProfile.hpp"
+#include "utils/purePursuitProfileManager.hpp"
+
+namespace Pronounce {
+
+	// Test path
+	int testPathIndex;
+
+	// Right steal right
+	int rightHomeToGoalNeutralIndex;
+	int rightNeutralToMidNeutralIndex;
+	int midNeutralToRightAllianceIndex;
+	int midNeutralToMidHomeZoneIndex;
+	int rightNeutralToRightHomeIndex;
+
+	// Right awp right
+	int farRightHomeZoneToRightAllianceIndex;
+	int rightAllianceToRightHomeZoneIndex;
+
+	// Left steal left
+	int leftAllianceToLeftNeutralIndex;
+	int leftNeutralToMidNeutralIndex;
+	int midNeutralToLeftHomeZoneIndex;
+
+	// Skills
+	int rightNeutralToFarPlatformIndex;
+	int farPlatformToNearPlatformIndex;
+	int nearPlatformViaLeftNeutralToFarPlatformIndex;
+	int nearPlatformToMidIndex;
+
+	void autoPaths(PurePursuit purePursuit) {
+		// Default pure pursuit profile
+		PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0);
+		purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile);
+
+		// Test path
+		Path testPath = Path();
+
+		testPath.addPoint(0, 0);
+		testPath.addPoint(24, 0);
+		testPath.addPoint(24, 24);
+		testPath.addPoint(24, 48);
+		testPath.addPoint(-24, 48);
+		testPath.addPoint(0, 24);
+
+		testPathIndex = purePursuit.addPath(testPath);
+
+		Path rightNeutralToRightHomeZone;
+
+		rightNeutralToRightHomeZone.addPoint(105.7, 60);
+		rightNeutralToRightHomeZone.addPoint(105.7, 16);
+
+		rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone);
+
+		// Right Steal Right
+		Path rightHomeToGoalNeutral;
+
+		rightHomeToGoalNeutral.addPoint(105.7, 16);
+		rightHomeToGoalNeutral.addPoint(105.7, 61);
+
+		rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral);
+
+		Path rightNeutralToMidNeutral;
+
+		rightNeutralToMidNeutral.addPoint(105.7, 62);
+		rightNeutralToMidNeutral.addPoint(75.3, 40);
+		rightNeutralToMidNeutral.addPoint(60.3, 65);
+
+		rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral);
+
+		Path midNeutralToRightAlliance;
+
+		midNeutralToRightAlliance.addPoint(70.3, 65);
+		midNeutralToRightAlliance.addPoint(120.1, 28);
+
+		midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance);
+
+		Path midNeutralToMidHomeZone;
+
+		midNeutralToMidHomeZone.addPoint(70.3, 70.3);
+		midNeutralToMidHomeZone.addPoint(70.3, 36);
+
+		midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone);
+
+		Path farRightHomeZoneToRightAlliance;
+
+		farRightHomeZoneToRightAlliance.addPoint(127.9, 16);
+		farRightHomeZoneToRightAlliance.addPoint(127.9, 24);
+
+		farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance);
+
+		Path rightAllianceToRightHomeZone;
+
+		rightAllianceToRightHomeZone.addPoint(127.9, 24);
+		rightAllianceToRightHomeZone.addPoint(105.7, 16);
+
+		rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone);
+
+		Path leftAllianceToLeftNeutral;
+
+		leftAllianceToLeftNeutral.addPoint(29, 11.4);
+		leftAllianceToLeftNeutral.addPoint(32, 67);
+
+		leftAllianceToLeftNeutralIndex = purePursuit.addPath(leftAllianceToLeftNeutral);
+
+		Path leftNeutralToMidNeutral;
+
+		leftNeutralToMidNeutral.addPoint(32, 67);
+		leftNeutralToMidNeutral.addPoint(65.3, 40);
+		leftNeutralToMidNeutral.addPoint(70.3, 65);
+
+		leftNeutralToMidNeutralIndex = purePursuit.addPath(leftNeutralToMidNeutral);
+
+		// mid neutral to mid home zone
+
+		Path rightNeutralToFarPlatform;
+
+		rightNeutralToFarPlatform.addPoint(105.7, 61);
+		rightNeutralToFarPlatform.addPoint(75, 76.5);
+		rightNeutralToFarPlatform.addPoint(75, 100);
+		rightNeutralToFarPlatform.addPoint(60.3, 115);
+
+		rightNeutralToFarPlatformIndex = purePursuit.addPath(rightNeutralToFarPlatform);
+
+		Path farPlatformToNearPlatform;
+
+		farPlatformToNearPlatform.addPoint(70.3, 107);
+		farPlatformToNearPlatform.addPoint(60, 70.3);
+		farPlatformToNearPlatform.addPoint(58.6, 64.1);
+		farPlatformToNearPlatform.addPoint(58.6, 45);
+		farPlatformToNearPlatform.addPoint(70.3, 30.7);
+
+		farPlatformToNearPlatformIndex = purePursuit.addPath(farPlatformToNearPlatform);
+
+		Path nearPlatformViaLeftNeutralToFarPlatform;
+
+		nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 30.7);
+		nearPlatformViaLeftNeutralToFarPlatform.addPoint(35, 61);
+		nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 115);
+
+		nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit.addPath(nearPlatformViaLeftNeutralToFarPlatform);
+
+		Path nearPlatformToMid;
+
+		nearPlatformToMid.addPoint(70.3, 115);
+		nearPlatformToMid.addPoint(70.3, 70.3);
+
+		nearPlatformToMidIndex = purePursuit.addPath(nearPlatformToMid);
+	}
+
+} // Namespace Prononce
diff --git a/include/main.h b/include/main.h
@@ -47,6 +47,7 @@
  * User defined imports
  */
 
+#include "autoPaths.hpp"
 #include "defines.h"
 
 // Auton

diff --git a/src/main.cpp b/src/main.cpp
@@ -55,30 +55,7 @@ bool preDriverTasksDone = false;
 
 int driverMode = 0;
 
-// Test path
-int testPathIndex;
-
-// Right steal right
-int rightHomeToGoalNeutralIndex;
-int rightNeutralToMidNeutralIndex;
-int midNeutralToRightAllianceIndex;
-int midNeutralToMidHomeZoneIndex;
-int rightNeutralToRightHomeIndex;
-
-// Right awp right
-int farRightHomeZoneToRightAllianceIndex;
-int rightAllianceToRightHomeZoneIndex;
-
-// Left steal left
-int leftAllianceToLeftNeutralIndex;
-int leftNeutralToMidNeutralIndex;
-int midNeutralToLeftHomeZoneIndex;
-
-// Skills
-int rightNeutralToFarPlatformIndex;
-int farPlatformToNearPlatformIndex;
-int nearPlatformViaLeftNeutralToFarPlatformIndex;
-int nearPlatformToMidIndex;
+// SECTION Auton
 
 void flipOut() {
 	intakeButton.setButtonStatus(ButtonStatus::NEGATIVE);
@@ -345,7 +322,9 @@ int postAuton() {
 	return 0;
 }
 
+// !SECTION
 
+// SECTION INIT
 /**
  * Render thread to update items on the controller
  */
@@ -475,6 +454,7 @@ void initSelector() {
  * Initialize the logger for data collection after the match
  */
 void initLogger() {
+
 	Logger::setDefaultLogger(
 		std::make_shared<Logger>(
 			TimeUtilFactory::createDefault().getTimer(),
@@ -485,127 +465,6 @@ void initLogger() {
 	Logger::getDefaultLogger()->debug<std::string>("LOGGER: Logger initialized");
 }
 
-void autoPaths() {
-	// Default pure pursuit profile
-	PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0);
-	purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile);
-
-	// Test path
-	Path testPath = Path();
-
-	testPath.addPoint(0, 0);
-	testPath.addPoint(24, 0);
-	testPath.addPoint(24, 24);
-	testPath.addPoint(24, 48);
-	testPath.addPoint(-24, 48);
-	testPath.addPoint(0, 24);
-
-	testPathIndex = purePursuit.addPath(testPath);
-
-	Path rightNeutralToRightHomeZone;
-
-	rightNeutralToRightHomeZone.addPoint(105.7, 60);
-	rightNeutralToRightHomeZone.addPoint(105.7, 16);
-
-	rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone);
-
-	// Right Steal Right
-	Path rightHomeToGoalNeutral;
-
-	rightHomeToGoalNeutral.addPoint(105.7, 16);
-	rightHomeToGoalNeutral.addPoint(105.7, 61);
-
-	rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral);
-
-	Path rightNeutralToMidNeutral;
-
-	rightNeutralToMidNeutral.addPoint(105.7, 62);
-	rightNeutralToMidNeutral.addPoint(75.3, 40);
-	rightNeutralToMidNeutral.addPoint(60.3, 65);
-
-	rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral);
-
-	Path midNeutralToRightAlliance;
-
-	midNeutralToRightAlliance.addPoint(70.3, 65);
-	midNeutralToRightAlliance.addPoint(120.1, 28);
-
-	midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance);
-
-	Path midNeutralToMidHomeZone;
-
-	midNeutralToMidHomeZone.addPoint(70.3, 70.3);
-	midNeutralToMidHomeZone.addPoint(70.3, 36);
-
-	midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone);
-
-	Path farRightHomeZoneToRightAlliance;
-
-	farRightHomeZoneToRightAlliance.addPoint(127.9, 16);
-	farRightHomeZoneToRightAlliance.addPoint(127.9, 24);
-
-	farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance);
-
-	Path rightAllianceToRightHomeZone;
-
-	rightAllianceToRightHomeZone.addPoint(127.9, 24);
-	rightAllianceToRightHomeZone.addPoint(105.7, 16);
-
-	rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone);
-
-	Path leftAllianceToLeftNeutral;
-
-	leftAllianceToLeftNeutral.addPoint(29, 11.4);
-	leftAllianceToLeftNeutral.addPoint(32, 67);
-
-	leftAllianceToLeftNeutralIndex = purePursuit.addPath(leftAllianceToLeftNeutral);
-
-	Path leftNeutralToMidNeutral;
-
-	leftNeutralToMidNeutral.addPoint(32, 67);
-	leftNeutralToMidNeutral.addPoint(65.3, 40);
-	leftNeutralToMidNeutral.addPoint(70.3, 65);
-
-	leftNeutralToMidNeutralIndex = purePursuit.addPath(leftNeutralToMidNeutral);
-
-	// mid neutral to mid home zone
-
-	Path rightNeutralToFarPlatform;
-
-	rightNeutralToFarPlatform.addPoint(105.7, 61);
-	rightNeutralToFarPlatform.addPoint(75, 76.5);
-	rightNeutralToFarPlatform.addPoint(75, 100);
-	rightNeutralToFarPlatform.addPoint(60.3, 115);
-
-	rightNeutralToFarPlatformIndex = purePursuit.addPath(rightNeutralToFarPlatform);
-
-	Path farPlatformToNearPlatform;
-
-	farPlatformToNearPlatform.addPoint(70.3, 107);
-	farPlatformToNearPlatform.addPoint(60, 70.3);
-	farPlatformToNearPlatform.addPoint(58.6, 64.1);
-	farPlatformToNearPlatform.addPoint(58.6, 45);
-	farPlatformToNearPlatform.addPoint(70.3, 30.7);
-
-	farPlatformToNearPlatformIndex = purePursuit.addPath(farPlatformToNearPlatform);
-
-	Path nearPlatformViaLeftNeutralToFarPlatform;
-
-	nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 30.7);
-	nearPlatformViaLeftNeutralToFarPlatform.addPoint(35, 61);
-	nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 115);
-
-	nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit.addPath(nearPlatformViaLeftNeutralToFarPlatform);
-
-	Path nearPlatformToMid;
-
-	nearPlatformToMid.addPoint(70.3, 115);
-	nearPlatformToMid.addPoint(70.3, 70.3);
-
-	nearPlatformToMidIndex = purePursuit.addPath(nearPlatformToMid);
-
-}
-
 /**
  * Filter and apply the quadratic function.
  */
@@ -637,12 +496,14 @@ void initialize() {
 	initSensors();
 	initMotors();
 	initDrivetrain();
-	autoPaths();
+	autoPaths(purePursuit);
 	initController();
 	initLogger();
 	// initSelector();
 }
 
+// !SECTION
+// SECTION Disabled
 /**
  * Runs while the robot is disabled i.e. before and after match, between auton
  * and teleop period
@@ -663,6 +524,10 @@ void disabled() {
 
 }
 
+// !SECTION
+
+// SECTION Competition Initialize
+
 /**
  * Starts when connected to the field
  */
@@ -671,6 +536,10 @@ void competition_initialize() {
 
 }
 
+// !SECTION
+
+// SECTION Auton
+
 /**
  * Runs during the autonomous. NO user control
  */
@@ -683,6 +552,9 @@ void autonomous() {
 	postAuton();
 }
 
+// !SECTION
+
+// SECTION Operator Control
 
 /**
  * Runs during operator/teleop control
@@ -734,3 +606,5 @@ void opcontrol() {
 		pros::delay(10);
 	}
 }
+
+// !SECTION