Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -40,18 +40,27 @@ namespace frc {
  * `Calculate()` and to determine when the profile has completed via
  * `IsFinished()`.
  */
+template <class Distance>
 class TrapezoidProfile {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+
  public:
   class Constraints {
    public:
-    units::meters_per_second_t maxVelocity = 0_mps;
-    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+    Velocity_t maxVelocity{0};
+    Acceleration_t maxAcceleration{0};
   };
 
   class State {
    public:
-    units::meter_t position = 0_m;
-    units::meters_per_second_t velocity = 0_mps;
+    Distance_t position{0};
+    Velocity_t velocity{0};
     bool operator==(const State& rhs) const {
       return position == rhs.position && velocity == rhs.velocity;
     }
@@ -66,7 +75,7 @@ class TrapezoidProfile {
    * @param initial     The initial state (usually the current state).
    */
   TrapezoidProfile(Constraints constraints, State goal,
-                   State initial = State{0_m, 0_mps});
+                   State initial = State{Distance_t(0), Velocity_t(0)});
 
   TrapezoidProfile(const TrapezoidProfile&) = default;
   TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
@@ -86,7 +95,7 @@ class TrapezoidProfile {
    *
    * @param target The target distance.
    */
-  units::second_t TimeLeftUntil(units::meter_t target) const;
+  units::second_t TimeLeftUntil(Distance_t target) const;
 
   /**
    * Returns the total time the profile takes to reach the goal.
@@ -135,5 +144,6 @@ class TrapezoidProfile {
   units::second_t m_endFullSpeed;
   units::second_t m_endDeccel;
 };
-
 }  // namespace frc
+
+#include "TrapezoidProfile.inc"

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc

@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+namespace frc {
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+                                             State goal, State initial)
+    : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
+      m_constraints(constraints),
+      m_initial(Direct(initial)),
+      m_goal(Direct(goal)) {
+  if (m_initial.velocity > m_constraints.maxVelocity) {
+    m_initial.velocity = m_constraints.maxVelocity;
+  }
+
+  // Deal with a possibly truncated motion profile (with nonzero initial or
+  // final velocity) by calculating the parameters as if the profile began and
+  // ended at zero velocity
+  units::second_t cutoffBegin =
+      m_initial.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistBegin =
+      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+  units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+  Distance_t cutoffDistEnd =
+      cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+  // Now we can calculate the parameters as if it was a full trapezoid instead
+  // of a truncated one
+
+  Distance_t fullTrapezoidDist =
+      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+  units::second_t accelerationTime =
+      m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+  Distance_t fullSpeedDist =
+      fullTrapezoidDist -
+      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+  // Handle the case where the profile never reaches full speed
+  if (fullSpeedDist < Distance_t(0)) {
+    accelerationTime =
+        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+    fullSpeedDist = Distance_t(0);
+  }
+
+  m_endAccel = accelerationTime - cutoffBegin;
+  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+}
+
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
+  State result = m_initial;
+
+  if (t < m_endAccel) {
+    result.velocity += t * m_constraints.maxAcceleration;
+    result.position +=
+        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+  } else if (t < m_endFullSpeed) {
+    result.velocity = m_constraints.maxVelocity;
+    result.position += (m_initial.velocity +
+                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
+                           m_endAccel +
+                       m_constraints.maxVelocity * (t - m_endAccel);
+  } else if (t <= m_endDeccel) {
+    result.velocity =
+        m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+    units::second_t timeLeft = m_endDeccel - t;
+    result.position =
+        m_goal.position -
+        (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+            timeLeft;
+  } else {
+    result = m_goal;
+  }
+
+  return Direct(result);
+}
+
+template <class Distance>
+units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
+    Distance_t target) const {
+  Distance_t position = m_initial.position * m_direction;
+  Velocity_t velocity = m_initial.velocity * m_direction;
+
+  units::second_t endAccel = m_endAccel * m_direction;
+  units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+  if (target < position) {
+    endAccel *= -1.0;
+    endFullSpeed *= -1.0;
+    velocity *= -1.0;
+  }
+
+  endAccel = units::math::max(endAccel, 0_s);
+  endFullSpeed = units::math::max(endFullSpeed, 0_s);
+  units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
+  endDeccel = units::math::max(endDeccel, 0_s);
+
+  const Acceleration_t acceleration = m_constraints.maxAcceleration;
+  const Acceleration_t decceleration = -m_constraints.maxAcceleration;
+
+  Distance_t distToTarget = units::math::abs(target - position);
+
+  if (distToTarget < Distance_t(1e-6)) {
+    return 0_s;
+  }
+
+  Distance_t accelDist =
+      velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+  Velocity_t deccelVelocity;
+  if (endAccel > 0_s) {
+    deccelVelocity = units::math::sqrt(
+        units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
+  } else {
+    deccelVelocity = velocity;
+  }
+
+  Distance_t deccelDist =
+      deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+  deccelDist = units::math::max(deccelDist, Distance_t(0));
+
+  Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+  if (accelDist > distToTarget) {
+    accelDist = distToTarget;
+    fullSpeedDist = Distance_t(0);
+    deccelDist = Distance_t(0);
+  } else if (accelDist + fullSpeedDist > distToTarget) {
+    fullSpeedDist = distToTarget - accelDist;
+    deccelDist = Distance_t(0);
+  } else {
+    deccelDist = distToTarget - fullSpeedDist - accelDist;
+  }
+
+  units::second_t accelTime =
+      (-velocity + units::math::sqrt(units::math::abs(
+                       velocity * velocity + 2 * acceleration * accelDist))) /
+      acceleration;
+
+  units::second_t deccelTime =
+      (-deccelVelocity +
+       units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
+                                          2 * decceleration * deccelDist))) /
+      decceleration;
+
+  units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+  return accelTime + fullSpeedTime + deccelTime;
+}
+}  // namespace frc