diff --git a/include/drivetrain.hpp b/include/drivetrain.hpp
index 66dbff5..28e691e 100644
--- a/include/drivetrain.hpp
+++ b/include/drivetrain.hpp
@@ -43,6 +43,11 @@ class drivetrain {
 	const float max_wheels_ang_vel_scaled;
 	const float min_wheels_ang_accel;
 	const float timestep;
+	const float max_robot_lin_vel_scaled;
+	const float max_robot_ang_vel;
+	const float max_robot_ang_vel_scaled;
+	const float max_robot_ang_accel_scaled;
+	const float max_robot_lin_accel_scaled;
 
 public:
 	/**
@@ -72,14 +77,9 @@ class drivetrain {
                        pros::Rotation& horizontal_wheel_,
                        pros::Imu& imu_,
                        pose Pose_);
-
-	const float max_robot_lin_vel_scaled;
-	const float max_robot_ang_vel;
-	const float max_robot_ang_vel_scaled;
-	const float max_robot_ang_accel_scaled;
+	
 	Localization localization;
-	mp LinearMP;
-	mp AngularMP;
+
 	/**
 	 * @brief Calculates maximum angular velocity for a given acceleration
 	 *
@@ -345,7 +345,8 @@ class drivetrain {
 	 *
 	 * @param[in] distance Distance to travel in meters (positive = forward, negative = backward)
 	 */
-	void linear_mp(const float distance, bool log = false);
+	void linear_mp(const float distance, bool log = false, const float percent_of_max_velocity = 100.f, const float percent_of_max_acceleration = 100.f);
+
 	
 	/**
 	 * @brief Executes an angular motion profile to rotate a specified angle
@@ -357,7 +358,8 @@ class drivetrain {
 	 *
 	 * @param[in] angle Angle to rotate in radians (positive = counterclockwise, negative = clockwise)
 	 */
-	void angular_mp(const float angle, bool log = false);
+	void angular_mp(const float angle, bool log = false, const float percent_of_max_velocity = 100.f, const float percent_of_max_acceleration = 100.f);
+
 	
 	/**
 	 * @brief Moves to a target pose using motion profiles
diff --git a/include/motion-profiler.hpp b/include/motion-profiler.hpp
index dc9430b..ed9491a 100644
--- a/include/motion-profiler.hpp
+++ b/include/motion-profiler.hpp
@@ -24,87 +24,93 @@
  */
 class mp {
 private:
+	/**
+	 * @brief Generates a triangular motion profile velocity
+	 *
+	 * Creates a triangular velocity profile that accelerates to a peak
+	 * velocity then decelerates to zero, without a constant velocity phase.
+	 * This is used when the distance is too short to reach maximum velocity.
+	 * The profile consists of two phases: acceleration and deceleration.
+	 *
+	 * @param[in] time Current time since profile start in seconds
+	 * @param[in] distance Total distance to travel in appropriate units
+	 *
+	 * @return Velocity at the specified time in appropriate units
+	 */
+	float triangular_motion_profile(float time);
+
+	/**
+	 * @brief Generates a trapezoidal motion profile velocity
+	 *
+	 * Creates a trapezoidal velocity profile with three phases: acceleration
+	 * to maximum velocity, constant velocity, and deceleration to zero.
+	 * This is used when the distance is sufficient to reach and maintain
+	 * maximum velocity for a period.
+	 *
+	 * @param[in] time Current time since profile start in seconds
+	 * @param[in] distance Total distance to travel in appropriate units
+	 *
+	 * @return Velocity at the specified time in appropriate units
+	 */
+	float trapezoidal_motion_profile(float time);
 
 
 public:
-    /**
-     * @brief Constructs a motion profiler with constraints
-     *
-     * Initializes the motion profiler with maximum acceleration and velocity
-     * limits. These constraints determine the shape and duration of generated
-     * motion profiles.
-     *
-     * @param[in] max_acceleration_ Maximum allowed acceleration in appropriate units
-     * @param[in] max_velocity_ Maximum allowed velocity in appropriate units
-     */
-    mp(const double max_acceleration_,
-       const double max_velocity_)
-        : max_acceleration(max_acceleration_),
-          max_velocity(max_velocity_)
-    {}
-    const double max_acceleration;
-    const double max_velocity;
-    double end_time;
-    
-    /**
-     * @brief Checks if the motion profile has completed
-     *
-     * Determines whether the motion profile has finished executing by
-     * comparing the current time to the calculated end time of the profile.
-     *
-     * @param[in] time Current time since profile start in seconds
-     *
-     * @return True if time >= end_time, false otherwise
-     */
-    bool profileFinished(double time);
-    
-    /**
-     * @brief Gets the velocity at a given time for a specified distance
-     *
-     * Calculates the velocity at a specific time point in the motion profile
-     * for a given total distance. Automatically selects between triangular
-     * and trapezoidal profiles based on whether the distance allows reaching
-     * maximum velocity. The end_time member is updated during calculation.
-     *
-     * @param[in] time Current time since profile start in seconds
-     * @param[in] distance Total distance to travel in appropriate units
-     *
-     * @return Velocity at the specified time in appropriate units
-     */
-    double velocity(double time, double distance);
-    
-    /**
-     * @brief Generates a triangular motion profile velocity
-     *
-     * Creates a triangular velocity profile that accelerates to a peak
-     * velocity then decelerates to zero, without a constant velocity phase.
-     * This is used when the distance is too short to reach maximum velocity.
-     * The profile consists of two phases: acceleration and deceleration.
-     *
-     * @param[in] time Current time since profile start in seconds
-     * @param[in] distance Total distance to travel in appropriate units
-     *
-     * @return Velocity at the specified time in appropriate units
-     */
-    double triangular_motion_profile(double time, double distance);
+	/**
+	 * @brief Constructs a motion profiler with constraints
+	 *
+	 * Initializes the motion profiler with maximum acceleration and velocity
+	 * limits. These constraints determine the shape and duration of generated
+	 * motion profiles.
+	 *
+	 * @param[in] max_acceleration_ Maximum allowed acceleration in appropriate units
+	 * @param[in] max_velocity_ Maximum allowed velocity in appropriate units
+	 */
+	mp(const float max_acceleration_,
+	const float max_velocity_,
+	const float distance_)
+		: max_acceleration(max_acceleration_),
+		max_velocity(max_velocity_),
+		distance(distance_),
+		end_time(2* distance_ / sqrt(distance_ * max_acceleration_)),
+		t_4(max_velocity_ / max_acceleration_),
+		t_5(max_velocity_ / max_acceleration_ + (distance_ - (1.f / 4.f * max_velocity_ / max_acceleration_ * max_velocity_)) / max_velocity_),
+		b(distance - pow(max_velocity_, 2) / max_acceleration_)
+	{}
+	const float max_acceleration;
+	const float max_velocity;
+	const float end_time;
+	const float distance;
     
-    /**
-     * @brief Generates a trapezoidal motion profile velocity
-     *
-     * Creates a trapezoidal velocity profile with three phases: acceleration
-     * to maximum velocity, constant velocity, and deceleration to zero.
-     * This is used when the distance is sufficient to reach and maintain
-     * maximum velocity for a period.
-     *
-     * @param[in] time Current time since profile start in seconds
-     * @param[in] distance Total distance to travel in appropriate units
-     *
-     * @return Velocity at the specified time in appropriate units
-     */
-    double trapezoidal_motion_profile(double time, double distance);
-};
-
-
+	const float t_4;
+    const float t_5;
+    const float b;
+	/**
+	 * @brief Checks if the motion profile has completed
+	 *
+	 * Determines whether the motion profile has finished executing by
+	 * comparing the current time to the calculated end time of the profile.
+	 *
+	 * @param[in] time Current time since profile start in seconds
+	 *
+	 * @return True if time >= end_time, false otherwise
+	 */
+	bool profileFinished(float time);
 
+	/**
+	 * @brief Gets the velocity at a given time for a specified distance
+	 *
+	 * Calculates the velocity at a specific time point in the motion profile
+	 * for a given total distance. Automatically selects between triangular
+	 * and trapezoidal profiles based on whether the distance allows reaching
+	 * maximum velocity. The end_time member is updated during calculation.
+	 *
+	 * @param[in] time Current time since profile start in seconds
+	 * @param[in] distance Total distance to travel in appropriate units
+	 *
+	 * @return Velocity at the specified time in appropriate units
+	 */
+	float velocity(float time);
+};
 
 #endif // MOTION_PROFILER_HPP
diff --git a/src/drivetrain.cpp b/src/drivetrain.cpp
index b8c04ba..08763e7 100644
--- a/src/drivetrain.cpp
+++ b/src/drivetrain.cpp
@@ -54,8 +54,7 @@ drivetrain::drivetrain(const wheels<std::reference_wrapper<pros::Motor>>& motors
                                (trackwidth_length_ / 12.f * (max_wheels_ang_vel_scaled * wheel_radius * 2.f))),
       max_robot_ang_accel_scaled(((wheelbase_length_ + trackwidth_length_) / 24.f * (min_wheels_ang_accel * decimal_of_max_acceleration * wheel_radius * 4.f)) +
                                  (trackwidth_length_ / 12.f * (min_wheels_ang_accel * decimal_of_max_acceleration * wheel_radius * 2.f))),
-      LinearMP((min_wheels_ang_accel * decimal_of_max_acceleration * wheel_radius), (max_robot_lin_vel_scaled)),
-      AngularMP((max_robot_ang_accel_scaled), (max_robot_ang_vel_scaled)),
+	  max_robot_lin_accel_scaled(min_wheels_ang_accel * decimal_of_max_acceleration * wheel_radius),
       timestep(timestep_)
 {}
 
@@ -201,18 +200,19 @@ void drivetrain::move_differential_robot_vels_ramsete(std::vector<differentialVe
         pros::delay(static_cast<int>(timestep*1000.f));
 	}
 }
-
-void drivetrain::linear_mp(const float distance, bool log) {
+  
+void drivetrain::linear_mp(const float distance, bool log, const float percent_of_max_velocity, const float percent_of_max_acceleration) {
     float time = 0.f;
     bool start = true;
+    mp linearMP(max_robot_lin_accel_scaled, max_robot_lin_vel_scaled, distance);
 	std::optional<std::vector<float>> linear_velocities;
     if (log) {
         linear_velocities.emplace();
-        linear_velocities->reserve(500);
+        linear_velocities->reserve(static_cast<int>(std::round(linearMP.end_time / timestep)));
     }
 
-    while(!LinearMP.profileFinished(time) || start) {
-        float linear_velocity = LinearMP.velocity(time, distance);
+    while(!linearMP.profileFinished(time) || start) {
+        float linear_velocity = linearMP.velocity(time);
 		if (linear_velocities) {
 			linear_velocities->push_back(linear_velocity);
 		}
@@ -230,17 +230,19 @@ void drivetrain::linear_mp(const float distance, bool log) {
     motor_brakes();
 }
 
-void drivetrain::angular_mp(const float angle, bool log) {
+
+void drivetrain::angular_mp(const float angle, bool log, const float percent_of_max_velocity, const float percent_of_max_acceleration) {
     float time = 0.f;
     bool start = true;
-	std::optional<std::vector<float>> angular_velocities;
+    mp angularMP(max_robot_ang_accel_scaled, max_robot_ang_vel_scaled, angle);
+  	std::optional<std::vector<float>> angular_velocities;
     if (log) {
         angular_velocities.emplace();
-        angular_velocities->reserve(500);
+        angular_velocities->reserve(static_cast<int>(std::round(angularMP.end_time / timestep)));
     }
+    while(!angularMP.profileFinished(time) || start) {
 
-    while(!AngularMP.profileFinished(time) || start) {
-        float angular_velocity = AngularMP.velocity(time, angle);
+        float angular_velocity = angularMP.velocity(time);
 		if (angular_velocities) {
 			angular_velocities->push_back(angular_velocity);
 		}
@@ -252,7 +254,7 @@ void drivetrain::angular_mp(const float angle, bool log) {
     }
 	
 	if (angular_velocities) {
-		print_vector(*angular_velocities, "L");
+		print_vector(*angular_velocities, "A");
 	}
     
 	motor_brakes();
@@ -278,5 +280,3 @@ void drivetrain::mtp_mp(const pose desired_pose) {
 void drivetrain::mtp_mp_ramsete(const pose desired_pose) {
 
 }
-
-
diff --git a/src/motion-profiler.cpp b/src/motion-profiler.cpp
index 895805c..281ebd0 100644
--- a/src/motion-profiler.cpp
+++ b/src/motion-profiler.cpp
@@ -8,44 +8,29 @@
  */
 #include "motion-profiler.hpp"
 
-bool mp::profileFinished(double time) {
+bool mp::profileFinished(float time) {
     return (time >= end_time);
 }
 
-double mp::velocity(double time, double distance) {
-    const double t_1 = max_velocity / max_acceleration;
-    const double c = max_velocity * t_1 / 2;
-    const double b = distance - 2 * c;
-    double velocity = 0.0;
-    if (b < 0) {
-        velocity = triangular_motion_profile(time, distance);
+float mp::velocity(float time) {
+    if (b < 0.f) {
+        return triangular_motion_profile(time);
     }
     else {
-        velocity = trapezoidal_motion_profile(time, distance);
+        return trapezoidal_motion_profile(time);
     }
-    return velocity;
 }
 
-double mp::triangular_motion_profile(double time, double distance) {
-    const double h = sqrt(distance * max_acceleration);
-    const double z = 2 * distance / h;
-    end_time = z;
-    if (time < z / 2) {
+float mp::triangular_motion_profile(float time) {
+    if (time < end_time / 2.f) {
         return (max_acceleration * time);
     }
     else {
-        return (-max_acceleration * (time - z/2) + max_acceleration * z/2);
+        return (-max_acceleration * (time - end_time/2.f) + max_acceleration * end_time/2.f);
     }
 }
 
-double mp::trapezoidal_motion_profile(double time, double distance) {
-    const double h = sqrt(distance * max_acceleration);
-    const double z = 2 * distance / h;
-    const double u = 2 * max_velocity / max_acceleration;
-    const double w = (distance - (1.0 / 2.0 * u * max_velocity)) / max_velocity;
-    const double t_4 = u / 2;
-    const double t_5 = t_4 + w;
-    end_time = t_5 + t_4;
+float mp::trapezoidal_motion_profile(float time) {
     if (time < t_4) {
         return (max_acceleration * time);    
     }