implement asym trapezoidal motion profile

updated:
drive.hpp
util.h/cpp
set_pid.cpp

RELENTLESS-Lite/include/arylib/drive/drive.hpp

@@ -9,6 +9,21 @@
 #include "arylib/util.hpp"
 #include "pros/motors.h"
 
+#define VELOCITY_TO_VOLTS_LIN 196.001723856
+#define VELOCITY_TO_VOLTS_ANG 127 / 0.796332147963
+
+#define LINEAR_MAX_VEL 0.647953484803
+#define LINEAR_FWD_ACCEL 0.647953484803 / 26
+#define LINEAR_FWD_DECEL 0.647953484803 / 18
+#define LINEAR_REV_ACCEL 0.647953484803 / 18
+#define LINEAR_REV_DECEL 0.647953484803 / 32
+
+#define ANGULAR_MAX_VEL 0.796332147963
+#define ANGULAR_FWD_ACCEL 0.796332147963 / 40
+#define ANGULAR_FWD_DECEL 0.796332147963 / 40
+#define ANGULAR_REV_ACCEL 0.796332147963 / 40
+#define ANGULAR_REV_DECEL 0.796332147963 / 40
+
 using namespace ary;
 
 class Drive {
@@ -507,6 +522,16 @@ class Drive {
    */
   void set_drive_pid(double target, int speed, bool slew_on = false, bool toggle_heading = true);
 
+  /**
+   * Drives the robot forward using a trapezoidal motional profile
+   * 
+   * /param target
+   *  target value in inches
+   * /param endTimeout
+   *  timeout value
+  */
+  void set_profiled_drive(double target, int endTimeout);
+
   /**
    * Sets the robot to turn using PID.
    *

RELENTLESS-Lite/include/arylib/util.hpp

@@ -9,6 +9,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 #include <bits/stdc++.h>
 #include <stdio.h>
 #include <string.h>
+#include <vector>
 
 #include "api.h"
 
@@ -91,6 +92,8 @@ extern bool AUTON_RAN;
  */
 int sgn(double input);
 
+std::vector<double> trapezoidalMotionProfile(double target, double maxVel, double accel, double decel);
+
 /**
  * Returns true if the input is < 0
  */

RELENTLESS-Lite/src/arylib/drive/set_pid.cpp

@@ -5,6 +5,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
 
 #include "main.h"
+#include "globals.hpp"
 
 // Updates max speed
 void Drive::set_max_speed(int speed) {
@@ -112,3 +113,24 @@ void Drive::set_swing_pid(e_swing type, double target, int speed) {
   // Run task
   set_mode(SWING);
 }
+
+void Drive::set_profiled_drive(double target, int endTimeout) {
+  //kv: rpm -> voltage
+  //sf: in/ms -> rpm
+  int sign = ary::util::sgn(target);
+  target = fabs(target);
+  std::vector<double> profile;
+  // std::cout << "reached 1" << std::endl;
+  if(sign > 0) profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_FWD_ACCEL, LINEAR_FWD_DECEL);
+  else profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_REV_ACCEL, LINEAR_REV_DECEL);
+
+  for (int i = 0; i < profile.size(); i++)
+  {
+    globals::chassisRight.set_tank(profile[i] * VELOCITY_TO_VOLTS_LIN * sign, profile[i] * VELOCITY_TO_VOLTS_LIN * sign);
+    pros::delay(10);
+  }
+
+  globals::chassisRight.set_tank(0, 0);
+  globals::chassisRight.set_drive_brake(MOTOR_BRAKE_COAST);
+  pros::delay(endTimeout);
+}

RELENTLESS-Lite/src/arylib/util.cpp

@@ -147,5 +147,38 @@ double clip_num(double input, double max, double min) {
   return input;
 }
 
+std::vector<double> trapezoidalMotionProfile(double dist, double maxVel, double accel, double decel) {
+  double max = std::min(std::sqrt((2 * accel * decel * dist) / accel + decel), maxVel);
+  double accelTime = max / accel;
+  double decelTime = max / decel;
+  double coastDist = (dist / max) - (max / (2 * accel)) - (max / (2 * decel));
+  double coastTime = coastDist / max;
+  double totalTime = accelTime + decelTime + coastTime;
+  double vel = 0;
+  double diff;
+  std::vector<double> profile;
+
+  for (int i = 0; i < std::ceil(totalTime); i++)
+  {
+    if (i < std::floor(accelTime))
+    {
+      profile.push_back(vel);
+      vel += accel;
+    }
+
+    else if (i < coastTime + accelTime)
+    {
+      profile.push_back(max);
+    }
+
+    else
+    {
+      profile.push_back(vel);
+      vel -= decel;
+    }
+  }
+  return profile;
+}
+
 }  // namespace util
 }  // namespace ary