From 8f026fcba343a5263e5ac9bb79ba972a7be8c3e1 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Wed, 15 Oct 2025 19:53:00 -0400
Subject: [PATCH 01/12] Created header files

---
 include/motionprofiling/motion_profile.h      | 27 +++++++++++++++++++
 .../motion_profile_generator.h                | 20 ++++++++++++++
 2 files changed, 47 insertions(+)
 create mode 100644 include/motionprofiling/motion_profile.h
 create mode 100644 include/motionprofiling/motion_profile_generator.h

diff --git a/include/motionprofiling/motion_profile.h b/include/motionprofiling/motion_profile.h
new file mode 100644
index 0000000..4d8216e
--- /dev/null
+++ b/include/motionprofiling/motion_profile.h
@@ -0,0 +1,27 @@
+/**
+ * Represents a motion profile for a path
+ */
+class MotionProfile {
+public:
+    /**
+     * Constructs a MotionProfile with specified parameters.
+     * 
+     * @param totalDistance The total distance to be covered by the motion profile. (Arc length)
+     * @param maxVelocity The maximum velocity to be reached during the motion profile.
+     * @param endAccelerationDistance The distance to target where initial acceleration stops.
+     * @param startDecelerationDistance The distance to target where deceleration begins.
+     */
+    MotionProfile(double totalDistance, 
+            double maxVelocity, 
+            double endAccelerationDistance, 
+            double startDecelerationDistance);
+    
+    /**
+     * Gets the target velocity at a given position
+     * 
+     * @param position The current position
+     * @return The target velocity at the given position.
+     */
+    double getVelocityFromPosition(double position);
+
+};
\ No newline at end of file
diff --git a/include/motionprofiling/motion_profile_generator.h b/include/motionprofiling/motion_profile_generator.h
new file mode 100644
index 0000000..a4c0ea0
--- /dev/null
+++ b/include/motionprofiling/motion_profile_generator.h
@@ -0,0 +1,20 @@
+/**
+ * Generates a motion profile for a given set of parameters.
+ */
+class MotionProfileGenerator {
+public:
+    /**
+     * Constructs a MotionProfileGenerator.
+     */
+    MotionProfileGenerator();
+
+    /**
+     * Generates a motion profile based on the provided parameters.
+     * 
+     * @param waypoints An array of Pose objects representing the waypoints of the path.
+     * @param maxVelocity The maximum velocity to be reached during the motion profile.
+     * @param acceleration The acceleration to be used in the motion profile.
+     * @return A MotionProfile object representing the generated motion profile.
+     */
+    MotionProfile generateProfile(Pose[] waypoints, double maxVelocity, double acceleration);
+};
\ No newline at end of file

From 47cb494490cd1599ccf6cb530b7894a6139d0800 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Wed, 15 Oct 2025 20:09:19 -0400
Subject: [PATCH 02/12] Working on motion profile generator

---
 .../motion_profile_generator.h                |  7 +-----
 .../motion_profile_generator.cpp              | 24 +++++++++++++++++++
 2 files changed, 25 insertions(+), 6 deletions(-)
 create mode 100644 src/motionprofiling/motion_profile_generator.cpp

diff --git a/include/motionprofiling/motion_profile_generator.h b/include/motionprofiling/motion_profile_generator.h
index a4c0ea0..2ae0cc8 100644
--- a/include/motionprofiling/motion_profile_generator.h
+++ b/include/motionprofiling/motion_profile_generator.h
@@ -3,11 +3,6 @@
  */
 class MotionProfileGenerator {
 public:
-    /**
-     * Constructs a MotionProfileGenerator.
-     */
-    MotionProfileGenerator();
-
     /**
      * Generates a motion profile based on the provided parameters.
      * 
@@ -16,5 +11,5 @@ class MotionProfileGenerator {
      * @param acceleration The acceleration to be used in the motion profile.
      * @return A MotionProfile object representing the generated motion profile.
      */
-    MotionProfile generateProfile(Pose[] waypoints, double maxVelocity, double acceleration);
+    static MotionProfile generateProfile(Pose[] waypoints, double maxVelocity, double acceleration);
 };
\ No newline at end of file
diff --git a/src/motionprofiling/motion_profile_generator.cpp b/src/motionprofiling/motion_profile_generator.cpp
new file mode 100644
index 0000000..fde1567
--- /dev/null
+++ b/src/motionprofiling/motion_profile_generator.cpp
@@ -0,0 +1,24 @@
+#include "motion_profile_generator.h"
+#include "motion_profile.h"
+#include "pose.h"
+#include "math.h"
+
+MotionProfileGenerator::MotionProfileGenerator() {}
+
+MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) {
+    double totalDistance = 0.0;
+    for (size_t i = 1; i < waypoints.size(); i++) {
+        totalDistance += math::hypot(
+            waypoints[i].x - waypoints[i-1].x, waypoints[i].y - waypoints[i-1].y);
+    }
+
+    double timeToAccelerate = maxVelocity / acceleration;
+
+    double endAccelerationDistance = maxVelocity * timeToAccelerate / 2.0;
+    double startDecelerationDistance = totalDistance - endAccelerationDistance;
+
+    MotionProfile profile = MotionProfile(
+        totalDistance, maxVelocity, endAccelerationDistance, startDecelerationDistance);
+
+    return profile;
+}
\ No newline at end of file

From 5d567fc34f2850f986d1df1e01f18511ffc882c3 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Mon, 20 Oct 2025 21:10:26 -0400
Subject: [PATCH 03/12] Made motion profile better

---
 include/motionprofiling/motion_profile.h | 18 +++++---
 src/motionprofiling/motion_profile.cpp   | 56 ++++++++++++++++++++++++
 2 files changed, 68 insertions(+), 6 deletions(-)
 create mode 100644 src/motionprofiling/motion_profile.cpp

diff --git a/include/motionprofiling/motion_profile.h b/include/motionprofiling/motion_profile.h
index 4d8216e..99920c3 100644
--- a/include/motionprofiling/motion_profile.h
+++ b/include/motionprofiling/motion_profile.h
@@ -6,12 +6,12 @@ class MotionProfile {
     /**
      * Constructs a MotionProfile with specified parameters.
      * 
-     * @param totalDistance The total distance to be covered by the motion profile. (Arc length)
+     * @param distanceMap A map of Pose objects to their corresponding accumulated distances.
      * @param maxVelocity The maximum velocity to be reached during the motion profile.
-     * @param endAccelerationDistance The distance to target where initial acceleration stops.
-     * @param startDecelerationDistance The distance to target where deceleration begins.
+     * @param endAccelerationDistance The distance along the path where initial acceleration stops.
+     * @param startDecelerationDistance The distance along the path where deceleration begins.
      */
-    MotionProfile(double totalDistance, 
+    MotionProfile(std::map<Pose, double> distanceMap,
             double maxVelocity, 
             double endAccelerationDistance, 
             double startDecelerationDistance);
@@ -19,9 +19,15 @@ class MotionProfile {
     /**
      * Gets the target velocity at a given position
      * 
-     * @param position The current position
+     * @param pose The Pose object representing the current position.
      * @return The target velocity at the given position.
      */
-    double getVelocityFromPosition(double position);
+    double getVelocityFromPosition(Pose pose);
 
+private:
+    double totalDistance;
+    double maxVelocity;
+    double endAccelerationDistance;
+    double startDecelerationDistance;
+    std::map<Pose, double> distanceMap;
 };
\ No newline at end of file
diff --git a/src/motionprofiling/motion_profile.cpp b/src/motionprofiling/motion_profile.cpp
new file mode 100644
index 0000000..fab76fe
--- /dev/null
+++ b/src/motionprofiling/motion_profile.cpp
@@ -0,0 +1,56 @@
+#include "motion_profile.h"
+
+MotionProfile::MotionProfile(std::map<Pose, double> distanceMap,
+        double maxVelocity, 
+        double endAccelerationDistance, 
+        double startDecelerationDistance) {
+
+    for (auto distancePair : distanceMap) {
+        this.totalDistance += distancePair.second;
+    }
+    this.distanceMap = distanceMap;
+    this.maxVelocity = maxVelocity;
+    this.endAccelerationDistance = endAccelerationDistance;
+    this.startDecelerationDistance = startDecelerationDistance;
+}
+
+double MotionProfile::getVelocityFromPosition(Pose pose) {
+    /* Find the closest pose in the distance map */
+    Pose closestPose;
+    if (distanceMap.contains(pose)) {
+        closestPose = pose;
+    } else {
+        double closestDistance = std::numeric_limits<double>::max();
+        for (auto distancePair : distanceMap) {
+            double dist = math::hypot(
+                pose.x - distancePair.first.x,
+                pose.y - distancePair.first.y);
+            if (dist < closestDistance) {
+                closestDistance = dist;
+                closestPose = distancePair.first;
+            }
+        }
+    }
+
+    /* Determine velocity based on distance to target */
+    double distanceAccumulated = distanceMap[closestPose];
+
+    // Same for acceleration and deceleration distances
+    double accelerationDistance = endAccelerationDistance;
+
+    // Note: Using v / vmax = sqrt(d / dmax)
+
+    if (distanceAccumulated <= endAccelerationDistance) {
+        // Accelerating
+        double velocity = maxVelocity * sqrt(distanceAccumulated / accelerationDistance);
+        return std::min(velocity, maxVelocity);
+    } else if (distanceAccumulated >= startDecelerationDistance) {
+        // Decelerating
+        double distanceToTarget = totalDistance - distanceAccumulated;
+        double velocity = maxVelocity * sqrt(distanceToTarget / accelerationDistance);
+        return std::min(velocity, maxVelocity);
+    } else {
+        // Cruising
+        return maxVelocity;
+    }
+}
\ No newline at end of file

From fc8fb009deed7f4ae5aa0f19c61f025bdb5654f9 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Mon, 20 Oct 2025 21:15:20 -0400
Subject: [PATCH 04/12] Working to fix stuff

---
 include/motionprofiling/motion_profile.h         | 3 ++-
 src/motionprofiling/motion_profile.cpp           | 5 ++---
 src/motionprofiling/motion_profile_generator.cpp | 1 +
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/include/motionprofiling/motion_profile.h b/include/motionprofiling/motion_profile.h
index 99920c3..88e8f89 100644
--- a/include/motionprofiling/motion_profile.h
+++ b/include/motionprofiling/motion_profile.h
@@ -12,6 +12,7 @@ class MotionProfile {
      * @param startDecelerationDistance The distance along the path where deceleration begins.
      */
     MotionProfile(std::map<Pose, double> distanceMap,
+            double totalDistance,
             double maxVelocity, 
             double endAccelerationDistance, 
             double startDecelerationDistance);
@@ -25,9 +26,9 @@ class MotionProfile {
     double getVelocityFromPosition(Pose pose);
 
 private:
+    std::map<Pose, double> distanceMap;
     double totalDistance;
     double maxVelocity;
     double endAccelerationDistance;
     double startDecelerationDistance;
-    std::map<Pose, double> distanceMap;
 };
\ No newline at end of file
diff --git a/src/motionprofiling/motion_profile.cpp b/src/motionprofiling/motion_profile.cpp
index fab76fe..3856f49 100644
--- a/src/motionprofiling/motion_profile.cpp
+++ b/src/motionprofiling/motion_profile.cpp
@@ -1,14 +1,13 @@
 #include "motion_profile.h"
 
 MotionProfile::MotionProfile(std::map<Pose, double> distanceMap,
+        double totalDistance,
         double maxVelocity, 
         double endAccelerationDistance, 
         double startDecelerationDistance) {
 
-    for (auto distancePair : distanceMap) {
-        this.totalDistance += distancePair.second;
-    }
     this.distanceMap = distanceMap;
+    this.totalDistance = totalDistance;
     this.maxVelocity = maxVelocity;
     this.endAccelerationDistance = endAccelerationDistance;
     this.startDecelerationDistance = startDecelerationDistance;
diff --git a/src/motionprofiling/motion_profile_generator.cpp b/src/motionprofiling/motion_profile_generator.cpp
index fde1567..c01c3d1 100644
--- a/src/motionprofiling/motion_profile_generator.cpp
+++ b/src/motionprofiling/motion_profile_generator.cpp
@@ -6,6 +6,7 @@
 MotionProfileGenerator::MotionProfileGenerator() {}
 
 MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) {
+    // TODO: Fix this to fulfill motion profile generation
     double totalDistance = 0.0;
     for (size_t i = 1; i < waypoints.size(); i++) {
         totalDistance += math::hypot(

From fcee7b12e3df5fe850c49d15224b7a266ff41bc3 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Mon, 20 Oct 2025 21:32:29 -0400
Subject: [PATCH 05/12] Fixed stuff to align with docs

---
 src/motionprofiling/motion_profile_generator.cpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/motionprofiling/motion_profile_generator.cpp b/src/motionprofiling/motion_profile_generator.cpp
index c01c3d1..e61d155 100644
--- a/src/motionprofiling/motion_profile_generator.cpp
+++ b/src/motionprofiling/motion_profile_generator.cpp
@@ -6,11 +6,17 @@
 MotionProfileGenerator::MotionProfileGenerator() {}
 
 MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) {
-    // TODO: Fix this to fulfill motion profile generation
+    std::vector<double> distances;
     double totalDistance = 0.0;
-    for (size_t i = 1; i < waypoints.size(); i++) {
+    for (size_t i = waypoints.size()-1; i > 0; i--) {
         totalDistance += math::hypot(
             waypoints[i].x - waypoints[i-1].x, waypoints[i].y - waypoints[i-1].y);
+        distances.push_back(totalDistance);
+    }
+
+    std::map<Pose, double> distanceMap;
+    for (size_t i = 0; i < waypoints.size(); i++) {
+        distanceMap[waypoints[i]] = distances[i];
     }
 
     double timeToAccelerate = maxVelocity / acceleration;
@@ -18,7 +24,7 @@ MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double m
     double endAccelerationDistance = maxVelocity * timeToAccelerate / 2.0;
     double startDecelerationDistance = totalDistance - endAccelerationDistance;
 
-    MotionProfile profile = MotionProfile(
+    MotionProfile profile = MotionProfile(distanceMap,
         totalDistance, maxVelocity, endAccelerationDistance, startDecelerationDistance);
 
     return profile;

From 5e5e7e727a7a56b4b9d4a20ba9b70e0d4390bb63 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Wed, 22 Oct 2025 19:05:28 -0400
Subject: [PATCH 06/12] Fixed generator

---
 src/motionprofiling/motion_profile_generator.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/motionprofiling/motion_profile_generator.cpp b/src/motionprofiling/motion_profile_generator.cpp
index e61d155..ed31d26 100644
--- a/src/motionprofiling/motion_profile_generator.cpp
+++ b/src/motionprofiling/motion_profile_generator.cpp
@@ -6,9 +6,9 @@
 MotionProfileGenerator::MotionProfileGenerator() {}
 
 MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) {
-    std::vector<double> distances;
+    std::vector<double> distances = {0.0};
     double totalDistance = 0.0;
-    for (size_t i = waypoints.size()-1; i > 0; i--) {
+    for (size_t i = 1; i < waypoints.size(); i++) {
         totalDistance += math::hypot(
             waypoints[i].x - waypoints[i-1].x, waypoints[i].y - waypoints[i-1].y);
         distances.push_back(totalDistance);

From 587cd3f11301f2c0a341637801f0a37d7b379d05 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Fri, 24 Oct 2025 18:22:07 -0400
Subject: [PATCH 07/12] Wrote something to follow a motion profile

---
 include/tank_drive.h |  1 +
 src/tank_drive.cpp   | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/include/tank_drive.h b/include/tank_drive.h
index 962f281..5626eb6 100644
--- a/include/tank_drive.h
+++ b/include/tank_drive.h
@@ -29,6 +29,7 @@ class TankDrive {
 	void turnToHeading(double targetHeadingDegrees);
 	void driveDistance(double distIn);
 	void driveToPoint(double targetX, double targetY);
+	void driveAlongPath(MotionProfile profile);
 
   private:
 	pros::MotorGroup leftMotorGroup;
diff --git a/src/tank_drive.cpp b/src/tank_drive.cpp
index 3be6b4f..f31e844 100644
--- a/src/tank_drive.cpp
+++ b/src/tank_drive.cpp
@@ -22,6 +22,8 @@ TankDrive::TankDrive(DrivebaseConfig config, pros::Controller &ctrl)
 											config.autonConstants.kITurn,
 											config.autonConstants.kDTurn);
 
+	pidCtrlPath = new PIDController<double>(1, 0, 0); // TODO: tune path PID
+
 	odom = new DrivebaseOdometry(config.brainside, config.batteryside,
 								 config.gearset,
 								 config.autonConstants.trackWidthIn);
@@ -31,6 +33,7 @@ TankDrive::TankDrive(DrivebaseConfig config, pros::Controller &ctrl)
 TankDrive::~TankDrive() {
 	delete pidCtrlMove;
 	delete pidCtrlTurn;
+	delete pidCtrlPath;
 	delete setPoint;
 	delete currentTask;
 }
@@ -85,6 +88,38 @@ void TankDrive::initAuton() {
 	currentTask = new pros::Task([this] { runAuton(); });
 }
 
+void TankDrive::driveAlongPath(MotionProfile profile) {
+	Pose currentPose;
+	Pose lastPose;
+	odom->getPose(&lastPose);
+
+	double currentVelocity = 0.0;
+	double lastTime = pros::millis();
+	while (true) {
+		odom->getPose(&currentPose);
+
+		// Calculate current velocity in some units per second
+		currentVelocity =
+			math::hypot(currentPose.x - lastPose.x,
+						currentPose.y - lastPose.y) /
+			((pros::millis() - lastTime) / 1000.0);
+
+		double maxMotorMag = (double)getInputExtremeForGearset(
+			(pros::motor_gearset_e)leftMotorGroup.get_gearing());
+
+		double targetVelocity = profile.getVelocityFromPosition(currentPose);
+		double calculatedPathPower = pidCtrlPath->compute(targetVelocity, currentVelocity);
+		calculatedPathPower = std::clamp(calculatedPathPower, -maxMotorMag, maxMotorMag);
+
+		// TODO: Add pure pursuit stuff here
+
+		leftMotorGroup.move(calculatedPathPower);
+		rightMotorGroup.move(calculatedPathPower);
+
+		pros::delay(20);
+	}
+}
+
 void TankDrive::driveToPose(Pose *targetPose) {
 	pidMode = COMBINED;
 	setPoint->x = targetPose->x;

From a0911ed421897c51dfe3efdf55854d1a9e7e4c73 Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Sat, 25 Oct 2025 17:07:48 -0400
Subject: [PATCH 08/12] Fixed some problems

---
 include/motionprofiling/motion_profile_generator.h |  2 +-
 src/motionprofiling/motion_profile.cpp             | 12 ++++++------
 src/motionprofiling/motion_profile_generator.cpp   |  6 ++----
 3 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/include/motionprofiling/motion_profile_generator.h b/include/motionprofiling/motion_profile_generator.h
index 2ae0cc8..8841cfa 100644
--- a/include/motionprofiling/motion_profile_generator.h
+++ b/include/motionprofiling/motion_profile_generator.h
@@ -11,5 +11,5 @@ class MotionProfileGenerator {
      * @param acceleration The acceleration to be used in the motion profile.
      * @return A MotionProfile object representing the generated motion profile.
      */
-    static MotionProfile generateProfile(Pose[] waypoints, double maxVelocity, double acceleration);
+    static MotionProfile generateProfile(std::vector<Pose> waypoints, double maxVelocity, double acceleration);
 };
\ No newline at end of file
diff --git a/src/motionprofiling/motion_profile.cpp b/src/motionprofiling/motion_profile.cpp
index 3856f49..a78c37e 100644
--- a/src/motionprofiling/motion_profile.cpp
+++ b/src/motionprofiling/motion_profile.cpp
@@ -6,11 +6,11 @@ MotionProfile::MotionProfile(std::map<Pose, double> distanceMap,
         double endAccelerationDistance, 
         double startDecelerationDistance) {
 
-    this.distanceMap = distanceMap;
-    this.totalDistance = totalDistance;
-    this.maxVelocity = maxVelocity;
-    this.endAccelerationDistance = endAccelerationDistance;
-    this.startDecelerationDistance = startDecelerationDistance;
+    this->distanceMap = distanceMap;
+    this->totalDistance = totalDistance;
+    this->maxVelocity = maxVelocity;
+    this->endAccelerationDistance = endAccelerationDistance;
+    this->startDecelerationDistance = startDecelerationDistance;
 }
 
 double MotionProfile::getVelocityFromPosition(Pose pose) {
@@ -21,7 +21,7 @@ double MotionProfile::getVelocityFromPosition(Pose pose) {
     } else {
         double closestDistance = std::numeric_limits<double>::max();
         for (auto distancePair : distanceMap) {
-            double dist = math::hypot(
+            double dist = std::hypot(
                 pose.x - distancePair.first.x,
                 pose.y - distancePair.first.y);
             if (dist < closestDistance) {
diff --git a/src/motionprofiling/motion_profile_generator.cpp b/src/motionprofiling/motion_profile_generator.cpp
index ed31d26..fa062fa 100644
--- a/src/motionprofiling/motion_profile_generator.cpp
+++ b/src/motionprofiling/motion_profile_generator.cpp
@@ -3,13 +3,11 @@
 #include "pose.h"
 #include "math.h"
 
-MotionProfileGenerator::MotionProfileGenerator() {}
-
-MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) {
+MotionProfile MotionProfileGenerator::generateProfile(std::vector<Pose> waypoints, double maxVelocity, double acceleration) {
     std::vector<double> distances = {0.0};
     double totalDistance = 0.0;
     for (size_t i = 1; i < waypoints.size(); i++) {
-        totalDistance += math::hypot(
+        totalDistance += std::hypot(
             waypoints[i].x - waypoints[i-1].x, waypoints[i].y - waypoints[i-1].y);
         distances.push_back(totalDistance);
     }

From 1d846458a4d0004458a8b9005d40cb8dbb4c9ced Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Sat, 25 Oct 2025 17:13:46 -0400
Subject: [PATCH 09/12] Fixed compilation issues

---
 include/motionprofiling/motion_profile.h         | 3 +++
 include/tank_drive.h                             | 2 ++
 src/motionprofiling/motion_profile.cpp           | 1 +
 src/motionprofiling/motion_profile_generator.cpp | 3 +++
 src/tank_drive.cpp                               | 2 +-
 5 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/include/motionprofiling/motion_profile.h b/include/motionprofiling/motion_profile.h
index 88e8f89..c946c7d 100644
--- a/include/motionprofiling/motion_profile.h
+++ b/include/motionprofiling/motion_profile.h
@@ -1,3 +1,6 @@
+#include "pose.h"
+#include <map>
+
 /**
  * Represents a motion profile for a path
  */
diff --git a/include/tank_drive.h b/include/tank_drive.h
index 5626eb6..c383cc5 100644
--- a/include/tank_drive.h
+++ b/include/tank_drive.h
@@ -9,6 +9,7 @@
 #include "pose.h"
 #include "robot_config.h"
 #include "utils.h"
+#include "motionprofiling/motion_profile.h"
 
 /**
  * A tank drive drivebase. Contains functions for both manual and
@@ -44,6 +45,7 @@ class TankDrive {
 	// PID Controllers
 	PIDController<Pose> *pidCtrlMove;
 	PIDController<double> *pidCtrlTurn;
+	PIDController<double> *pidCtrlPath;
 
 	// Drive control methods
 	void tankDrive();
diff --git a/src/motionprofiling/motion_profile.cpp b/src/motionprofiling/motion_profile.cpp
index a78c37e..dc6d26f 100644
--- a/src/motionprofiling/motion_profile.cpp
+++ b/src/motionprofiling/motion_profile.cpp
@@ -1,4 +1,5 @@
 #include "motion_profile.h"
+#include <cmath>
 
 MotionProfile::MotionProfile(std::map<Pose, double> distanceMap,
         double totalDistance,
diff --git a/src/motionprofiling/motion_profile_generator.cpp b/src/motionprofiling/motion_profile_generator.cpp
index fa062fa..79c9adc 100644
--- a/src/motionprofiling/motion_profile_generator.cpp
+++ b/src/motionprofiling/motion_profile_generator.cpp
@@ -2,6 +2,9 @@
 #include "motion_profile.h"
 #include "pose.h"
 #include "math.h"
+#include <vector>
+#include <map>
+#include <cmath>
 
 MotionProfile MotionProfileGenerator::generateProfile(std::vector<Pose> waypoints, double maxVelocity, double acceleration) {
     std::vector<double> distances = {0.0};
diff --git a/src/tank_drive.cpp b/src/tank_drive.cpp
index f31e844..0f2c5de 100644
--- a/src/tank_drive.cpp
+++ b/src/tank_drive.cpp
@@ -100,7 +100,7 @@ void TankDrive::driveAlongPath(MotionProfile profile) {
 
 		// Calculate current velocity in some units per second
 		currentVelocity =
-			math::hypot(currentPose.x - lastPose.x,
+			std::hypot(currentPose.x - lastPose.x,
 						currentPose.y - lastPose.y) /
 			((pros::millis() - lastTime) / 1000.0);
 

From efd9f4f19cb563592e805fd69ff0ffcf10ce3f3d Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Thu, 30 Oct 2025 20:35:32 -0400
Subject: [PATCH 10/12] Fixed compilation issues

---
 include/motionprofiling/motion_profile.h           |  7 ++++++-
 include/motionprofiling/motion_profile_generator.h | 11 ++++++++++-
 include/pose.h                                     | 10 ++++++++++
 3 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/include/motionprofiling/motion_profile.h b/include/motionprofiling/motion_profile.h
index c946c7d..c938af1 100644
--- a/include/motionprofiling/motion_profile.h
+++ b/include/motionprofiling/motion_profile.h
@@ -1,3 +1,6 @@
+#ifndef MOTION_PROFILE_H
+#define MOTION_PROFILE_H
+
 #include "pose.h"
 #include <map>
 
@@ -34,4 +37,6 @@ class MotionProfile {
     double maxVelocity;
     double endAccelerationDistance;
     double startDecelerationDistance;
-};
\ No newline at end of file
+};
+
+#endif
\ No newline at end of file
diff --git a/include/motionprofiling/motion_profile_generator.h b/include/motionprofiling/motion_profile_generator.h
index 8841cfa..f626484 100644
--- a/include/motionprofiling/motion_profile_generator.h
+++ b/include/motionprofiling/motion_profile_generator.h
@@ -1,3 +1,10 @@
+#ifndef MOTION_PROFILE_GENERATOR_H
+#define MOTION_PROFILE_GENERATOR_H
+
+#include "motion_profile.h"
+#include "pose.h"
+#include <vector>
+
 /**
  * Generates a motion profile for a given set of parameters.
  */
@@ -12,4 +19,6 @@ class MotionProfileGenerator {
      * @return A MotionProfile object representing the generated motion profile.
      */
     static MotionProfile generateProfile(std::vector<Pose> waypoints, double maxVelocity, double acceleration);
-};
\ No newline at end of file
+};
+
+#endif
\ No newline at end of file
diff --git a/include/pose.h b/include/pose.h
index 1a14c04..9cc28da 100644
--- a/include/pose.h
+++ b/include/pose.h
@@ -1,6 +1,8 @@
 #ifndef POSE
 #define POSE
 
+#include <tuple>
+
 /**
  * A position on the field. Values are absolute, not relative to the field.
  */
@@ -10,4 +12,12 @@ struct Pose {
 	double theta;
 };
 
+/**
+ * Comparison operator for Pose to enable use as a map key.
+ * Compares lexicographically by x, then y, then theta.
+ */
+inline bool operator<(const Pose &a, const Pose &b) {
+	return std::tie(a.x, a.y, a.theta) < std::tie(b.x, b.y, b.theta);
+}
+
 #endif
\ No newline at end of file

From b0c5d0bad70787a9a671d43abd41a93002e8febf Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Thu, 30 Oct 2025 20:35:43 -0400
Subject: [PATCH 11/12] Fixed pose

---
 include/pose.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/pose.h b/include/pose.h
index 9cc28da..12aa639 100644
--- a/include/pose.h
+++ b/include/pose.h
@@ -14,7 +14,7 @@ struct Pose {
 
 /**
  * Comparison operator for Pose to enable use as a map key.
- * Compares lexicographically by x, then y, then theta.
+ * Compares by x, then y, then theta.
  */
 inline bool operator<(const Pose &a, const Pose &b) {
 	return std::tie(a.x, a.y, a.theta) < std::tie(b.x, b.y, b.theta);

From be7ed9767c4de9b26f73c9c535a941a782358b7a Mon Sep 17 00:00:00 2001
From: Rishi Ponnapalli <rishiponnapalli13@gmail.com>
Date: Wed, 5 Nov 2025 19:16:15 -0500
Subject: [PATCH 12/12] Removed tank drive stuff

---
 include/tank_drive.h |  2 --
 src/tank_drive.cpp   | 35 -----------------------------------
 2 files changed, 37 deletions(-)

diff --git a/include/tank_drive.h b/include/tank_drive.h
index c383cc5..6ec8de1 100644
--- a/include/tank_drive.h
+++ b/include/tank_drive.h
@@ -30,7 +30,6 @@ class TankDrive {
 	void turnToHeading(double targetHeadingDegrees);
 	void driveDistance(double distIn);
 	void driveToPoint(double targetX, double targetY);
-	void driveAlongPath(MotionProfile profile);
 
   private:
 	pros::MotorGroup leftMotorGroup;
@@ -45,7 +44,6 @@ class TankDrive {
 	// PID Controllers
 	PIDController<Pose> *pidCtrlMove;
 	PIDController<double> *pidCtrlTurn;
-	PIDController<double> *pidCtrlPath;
 
 	// Drive control methods
 	void tankDrive();
diff --git a/src/tank_drive.cpp b/src/tank_drive.cpp
index 0f2c5de..3be6b4f 100644
--- a/src/tank_drive.cpp
+++ b/src/tank_drive.cpp
@@ -22,8 +22,6 @@ TankDrive::TankDrive(DrivebaseConfig config, pros::Controller &ctrl)
 											config.autonConstants.kITurn,
 											config.autonConstants.kDTurn);
 
-	pidCtrlPath = new PIDController<double>(1, 0, 0); // TODO: tune path PID
-
 	odom = new DrivebaseOdometry(config.brainside, config.batteryside,
 								 config.gearset,
 								 config.autonConstants.trackWidthIn);
@@ -33,7 +31,6 @@ TankDrive::TankDrive(DrivebaseConfig config, pros::Controller &ctrl)
 TankDrive::~TankDrive() {
 	delete pidCtrlMove;
 	delete pidCtrlTurn;
-	delete pidCtrlPath;
 	delete setPoint;
 	delete currentTask;
 }
@@ -88,38 +85,6 @@ void TankDrive::initAuton() {
 	currentTask = new pros::Task([this] { runAuton(); });
 }
 
-void TankDrive::driveAlongPath(MotionProfile profile) {
-	Pose currentPose;
-	Pose lastPose;
-	odom->getPose(&lastPose);
-
-	double currentVelocity = 0.0;
-	double lastTime = pros::millis();
-	while (true) {
-		odom->getPose(&currentPose);
-
-		// Calculate current velocity in some units per second
-		currentVelocity =
-			std::hypot(currentPose.x - lastPose.x,
-						currentPose.y - lastPose.y) /
-			((pros::millis() - lastTime) / 1000.0);
-
-		double maxMotorMag = (double)getInputExtremeForGearset(
-			(pros::motor_gearset_e)leftMotorGroup.get_gearing());
-
-		double targetVelocity = profile.getVelocityFromPosition(currentPose);
-		double calculatedPathPower = pidCtrlPath->compute(targetVelocity, currentVelocity);
-		calculatedPathPower = std::clamp(calculatedPathPower, -maxMotorMag, maxMotorMag);
-
-		// TODO: Add pure pursuit stuff here
-
-		leftMotorGroup.move(calculatedPathPower);
-		rightMotorGroup.move(calculatedPathPower);
-
-		pros::delay(20);
-	}
-}
-
 void TankDrive::driveToPose(Pose *targetPose) {
 	pidMode = COMBINED;
 	setPoint->x = targetPose->x;