diff --git a/include/motionprofiling/motion_profile.h b/include/motionprofiling/motion_profile.h new file mode 100644 index 0000000..c938af1 --- /dev/null +++ b/include/motionprofiling/motion_profile.h @@ -0,0 +1,42 @@ +#ifndef MOTION_PROFILE_H +#define MOTION_PROFILE_H + +#include "pose.h" +#include + +/** + * 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 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 distanceMap; + double totalDistance; + double maxVelocity; + double endAccelerationDistance; + double startDecelerationDistance; +}; + +#endif \ 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..f626484 --- /dev/null +++ b/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 + +/** + * 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 waypoints, double maxVelocity, double acceleration); +}; + +#endif \ No newline at end of file diff --git a/include/pose.h b/include/pose.h index 1a14c04..12aa639 100644 --- a/include/pose.h +++ b/include/pose.h @@ -1,6 +1,8 @@ #ifndef POSE #define POSE +#include + /** * 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 \ No newline at end of file diff --git a/include/tank_drive.h b/include/tank_drive.h index 962f281..6ec8de1 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 diff --git a/src/motionprofiling/motion_profile.cpp b/src/motionprofiling/motion_profile.cpp new file mode 100644 index 0000000..dc6d26f --- /dev/null +++ b/src/motionprofiling/motion_profile.cpp @@ -0,0 +1,56 @@ +#include "motion_profile.h" +#include + +MotionProfile::MotionProfile(std::map 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::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; + } +} \ 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..79c9adc --- /dev/null +++ b/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 +#include +#include + +MotionProfile MotionProfileGenerator::generateProfile(std::vector waypoints, double maxVelocity, double acceleration) { + std::vector 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 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; +} \ No newline at end of file