MOTION PROFILES???

RELENTLESS/include/ary-lib/drive/drive.hpp

@@ -15,6 +15,21 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 #include "ary-lib/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 {
@@ -513,6 +528,16 @@ class Drive {
    */
   void set_drive(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_proifiled_drive(Drive& chassis, double target, int endTimeout);
+
   /**
    * Sets the robot to turn using PID.
    *

RELENTLESS/include/ary-lib/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"
 
@@ -101,6 +102,9 @@ bool is_reversed(double input);
  */
 double clip_num(double input, double max, double min);
 
+
+std::vector<double> trapezoidalMotionProfile(double target, double maxVel, double accel, double decel);
+
 /**
  * Is the SD card plugged in?
  */

RELENTLESS/src/ary-lib/drive/set_pid.cpp

@@ -112,3 +112,25 @@ void Drive::set_swing(e_swing type, double target, int speed) {
   // Run task
   set_mode(SWING);
 }
+
+void set_profiled_drive(Drive& chassis, double target, int endTimeout) {
+  //kv: rpm -> voltage
+  //sf: in/ms -> rpm
+  int sign = ary::util::signum(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++)
+  {
+    chassis.set_tank(profile[i] * VELOCITY_TO_VOLTS_LIN * sign, profile[i] * VELOCITY_TO_VOLTS_LIN * sign);
+    pros::delay(10);
+  }
+  
+  chassis.set_tank(0, 0);
+  chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
+  pros::delay(endTimeout);
+
+}

RELENTLESS/src/ary-lib/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