Feature: Motion profiling

include/motionprofiling/motion_profile.h

@@ -0,0 +1,42 @@
+#ifndef MOTION_PROFILE_H
+#define MOTION_PROFILE_H
+
+#include "pose.h"
+#include <map>
+
+/**
+ * Represents a motion profile for a path
+ */
+class MotionProfile {
+public:
+    /**
+     * Constructs a MotionProfile with specified parameters.
+     * 
+     * @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 along the path where initial acceleration stops.
+     * @param startDecelerationDistance The distance along the path where deceleration begins.
+     */
+    MotionProfile(std::map<Pose, double> distanceMap,
+            double totalDistance,
+            double maxVelocity, 
+            double endAccelerationDistance, 
+            double startDecelerationDistance);
+
+    /**
+     * Gets the target velocity at a given position
+     * 
+     * @param pose The Pose object representing the current position.
+     * @return The target velocity at the given position.
+     */
+    double getVelocityFromPosition(Pose pose);
+
+private:
+    std::map<Pose, double> distanceMap;
+    double totalDistance;
+    double maxVelocity;
+    double endAccelerationDistance;
+    double startDecelerationDistance;
+};
+
+#endif

include/motionprofiling/motion_profile_generator.h

@@ -0,0 +1,24 @@
+#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.
+ */
+class MotionProfileGenerator {
+public:
+    /**
+     * 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.
+     */
+    static MotionProfile generateProfile(std::vector<Pose> waypoints, double maxVelocity, double acceleration);
+};
+
+#endif

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 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

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

src/motionprofiling/motion_profile.cpp

@@ -0,0 +1,56 @@
+#include "motion_profile.h"
+#include <cmath>
+
+MotionProfile::MotionProfile(std::map<Pose, double> distanceMap,
+        double totalDistance,
+        double maxVelocity, 
+        double endAccelerationDistance, 
+        double startDecelerationDistance) {
+
+    this->distanceMap = distanceMap;
+    this->totalDistance = totalDistance;
+    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 = std::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;
+    }
+}

src/motionprofiling/motion_profile_generator.cpp

@@ -0,0 +1,32 @@
+#include "motion_profile_generator.h"
+#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};
+    double totalDistance = 0.0;
+    for (size_t i = 1; i < waypoints.size(); i++) {
+        totalDistance += std::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;
+
+    double endAccelerationDistance = maxVelocity * timeToAccelerate / 2.0;
+    double startDecelerationDistance = totalDistance - endAccelerationDistance;
+
+    MotionProfile profile = MotionProfile(distanceMap,
+        totalDistance, maxVelocity, endAccelerationDistance, startDecelerationDistance);
+
+    return profile;
+}