diff --git a/.styleguide b/.styleguide
index b93b522cef..00cc8eb7cf 100644
--- a/.styleguide
+++ b/.styleguide
@@ -34,6 +34,7 @@ includeOtherLibs {
   ^ntcore
   ^opencv2/
   ^support/
+  ^units/
   ^vision/
   ^wpi/
 }
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp b/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
new file mode 100644
index 0000000000..6026aa548d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrapezoidProfile.h"
+
+using namespace frc;
+
+TrapezoidProfile::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;
+  units::meter_t cutoffDistBegin =
+      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+  units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+  units::meter_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
+
+  units::meter_t fullTrapezoidDist =
+      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+  units::second_t accelerationTime =
+      m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+  units::meter_t fullSpeedDist =
+      fullTrapezoidDist -
+      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+  // Handle the case where the profile never reaches full speed
+  if (fullSpeedDist < 0_m) {
+    accelerationTime =
+        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+    fullSpeedDist = 0_m;
+  }
+
+  m_endAccel = accelerationTime - cutoffBegin;
+  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+}
+
+TrapezoidProfile::State TrapezoidProfile::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);
+}
+
+units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
+  units::meter_t position = m_initial.position * m_direction;
+  units::meters_per_second_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 units::meters_per_second_squared_t acceleration =
+      m_constraints.maxAcceleration;
+  const units::meters_per_second_squared_t decceleration =
+      -m_constraints.maxAcceleration;
+
+  units::meter_t distToTarget = units::math::abs(target - position);
+
+  if (distToTarget < 1e-6_m) {
+    return 0_s;
+  }
+
+  units::meter_t accelDist =
+      velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+  units::meters_per_second_t deccelVelocity;
+  if (endAccel > 0_s) {
+    deccelVelocity = units::math::sqrt(
+        units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
+  } else {
+    deccelVelocity = velocity;
+  }
+
+  units::meter_t deccelDist =
+      deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+  deccelDist = units::math::max(deccelDist, 0_m);
+
+  units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+  if (accelDist > distToTarget) {
+    accelDist = distToTarget;
+    fullSpeedDist = 0_m;
+    deccelDist = 0_m;
+  } else if (accelDist + fullSpeedDist > distToTarget) {
+    fullSpeedDist = distToTarget - accelDist;
+    deccelDist = 0_m;
+  } 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;
+}
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
new file mode 100644
index 0000000000..920bf22264
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/units.h>
+
+namespace frc {
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * Initialization:
+ * @code{.cpp}
+ * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
+ * double previousProfiledReference = initialReference;
+ * @endcode
+ *
+ * Run on update:
+ * @code{.cpp}
+ * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
+ *                                  previousProfiledReference};
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
+ * @endcode
+ *
+ * where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `Calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * Otherwise, a timer can be started to provide monotonic values for
+ * `Calculate()` and to determine when the profile has completed via
+ * `IsFinished()`.
+ */
+class TrapezoidProfile {
+ public:
+  class Constraints {
+   public:
+    units::meters_per_second_t maxVelocity = 0_mps;
+    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+  };
+
+  class State {
+   public:
+    units::meter_t position = 0_m;
+    units::meters_per_second_t velocity = 0_mps;
+    bool operator==(const State& rhs) const {
+      return position == rhs.position && velocity == rhs.velocity;
+    }
+    bool operator!=(const State& rhs) const { return !(*this == rhs); }
+  };
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   * @param initial     The initial state (usually the current state).
+   */
+  TrapezoidProfile(Constraints constraints, State goal,
+                   State initial = State{0_m, 0_mps});
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  State Calculate(units::second_t t) const;
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   */
+  units::second_t TimeLeftUntil(units::meter_t target) const;
+
+  /**
+   * Returns the total time the profile takes to reach the goal.
+   */
+  units::second_t TotalTime() const { return m_endDeccel; }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * The profile has reached the goal if the time since the profile started
+   * has exceeded the profile's total time.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
+
+ private:
+  /**
+   * Returns true if the profile inverted.
+   *
+   * The profile is inverted if goal position is less than the initial position.
+   *
+   * @param initial The initial state (usually the current state).
+   * @param goal    The desired state when the profile is complete.
+   */
+  static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  State Direct(const State& in) const {
+    State result = in;
+    result.position *= m_direction;
+    result.velocity *= m_direction;
+    return result;
+  }
+
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  int m_direction;
+
+  Constraints m_constraints;
+  State m_initial;
+  State m_goal;
+
+  units::second_t m_endAccel;
+  units::second_t m_endFullSpeed;
+  units::second_t m_endDeccel;
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/test/native/cpp/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/TrapezoidProfileTest.cpp
new file mode 100644
index 0000000000..ddd6a705ea
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/TrapezoidProfileTest.cpp
@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrapezoidProfile.h"  // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+static constexpr auto kDt = 10_ms;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+  if (val1 <= val2) {                            \
+    EXPECT_LE(val1, val2);                       \
+  } else {                                       \
+    EXPECT_NEAR_UNITS(val1, val2, eps);          \
+  }
+
+TEST(TrapezoidProfileTest, ReachesGoal) {
+  frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{3_m, 0_mps};
+  frc::TrapezoidProfile::State state;
+
+  for (int i = 0; i < 450; ++i) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
+  frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{12_m, 0_mps};
+
+  frc::TrapezoidProfile profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto lastPos = state.position;
+  for (int i = 0; i < 1600; ++i) {
+    if (i == 400) {
+      constraints.maxVelocity = 0.75_mps;
+    }
+
+    profile = frc::TrapezoidProfile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+    auto estimatedVel = (state.position - lastPos) / kDt;
+
+    if (i >= 400) {
+      // Since estimatedVel can have floating point rounding errors, we check
+      // whether value is less than or within an error delta of the new
+      // constraint.
+      EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
+
+      EXPECT_LE(state.velocity, constraints.maxVelocity);
+    }
+
+    lastPos = state.position;
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(TrapezoidProfileTest, Backwards) {
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile::State state;
+
+  for (int i = 0; i < 400; ++i) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile::State state;
+
+  for (int i = 0; i < 200; ++i) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_NE(state, goal);
+
+  goal = {0.0_m, 0.0_mps};
+  for (int i = 0; i < 550; ++i) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed
+TEST(TrapezoidProfileTest, TopSpeed) {
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{4_m, 0_mps};
+  frc::TrapezoidProfile::State state;
+
+  for (int i = 0; i < 200; ++i) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
+
+  for (int i = 0; i < 2000; ++i) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+  }
+  EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, TimingToCurrent) {
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile::State state;
+
+  for (int i = 0; i < 400; i++) {
+    frc::TrapezoidProfile profile{constraints, goal, state};
+    state = profile.Calculate(kDt);
+    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingToGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+
+  frc::TrapezoidProfile profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal && state == goal) {
+      // Expected value using for loop index is just an approximation since the
+      // time left in the profile doesn't increase linearly at the endpoints
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+      reachedGoal = true;
+    }
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+
+  frc::TrapezoidProfile profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal &&
+        (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+      reachedGoal = true;
+    }
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+
+  frc::TrapezoidProfile profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal && state == goal) {
+      // Expected value using for loop index is just an approximation since the
+      // time left in the profile doesn't increase linearly at the endpoints
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+      reachedGoal = true;
+    }
+  }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
+  using units::unit_cast;
+
+  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+
+  frc::TrapezoidProfile profile{constraints, goal};
+  auto state = profile.Calculate(kDt);
+
+  auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
+  bool reachedGoal = false;
+  for (int i = 0; i < 400; i++) {
+    profile = frc::TrapezoidProfile(constraints, goal, state);
+    state = profile.Calculate(kDt);
+    if (!reachedGoal &&
+        (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
+      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+      reachedGoal = true;
+    }
+  }
+}
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
new file mode 100644
index 0000000000..e6c7baea56
--- /dev/null
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
@@ -0,0 +1,294 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import java.util.Objects;
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * <p>While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * <p>Initialization:
+ * <pre><code>
+ * TrapezoidProfile.Constraints constraints =
+ *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+ * TrapezoidProfile.State previousProfiledReference =
+ *   new TrapezoidProfile.State(initialReference, 0.0);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ * <pre><code>
+ * TrapezoidProfile profile =
+ *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
+ * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for
+ * `calculate()` and to determine when the profile has completed via
+ * `isFinished()`.
+ */
+public class TrapezoidProfile {
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  private int m_direction;
+
+  private Constraints m_constraints;
+  private State m_initial;
+  private State m_goal;
+
+  private double m_endAccel;
+  private double m_endFullSpeed;
+  private double m_endDeccel;
+
+  public static class Constraints {
+    @SuppressWarnings("MemberName")
+    public double maxVelocity;
+    @SuppressWarnings("MemberName")
+    public double maxAcceleration;
+
+    public Constraints() {
+    }
+
+    public Constraints(double maxVelocity, double maxAcceleration) {
+      this.maxVelocity = maxVelocity;
+      this.maxAcceleration = maxAcceleration;
+    }
+  }
+
+  public static class State {
+    @SuppressWarnings("MemberName")
+    public double position;
+    @SuppressWarnings("MemberName")
+    public double velocity;
+
+    public State() {
+    }
+
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   * @param initial     The initial state (usually the current state).
+   */
+  public 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
+    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+    double 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
+
+    double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
+        + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
+        * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal        The desired state when the profile is complete.
+   */
+  public TrapezoidProfile(Constraints constraints, State goal) {
+    this(constraints, goal, new State(0, 0));
+  }
+
+  /**
+   * Calculate the correct position and velocity for the profile at a time t
+   * where the beginning of the profile was at time t = 0.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  @SuppressWarnings("ParameterName")
+  public State calculate(double t) {
+    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;
+      double 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);
+  }
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   */
+  public double timeLeftUntil(double target) {
+    double position = m_initial.position * m_direction;
+    double velocity = m_initial.velocity * m_direction;
+
+    double endAccel = m_endAccel * m_direction;
+    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+    if (target < position) {
+      endAccel = -endAccel;
+      endFullSpeed = -endFullSpeed;
+      velocity = -velocity;
+    }
+
+    endAccel = Math.max(endAccel, 0);
+    endFullSpeed = Math.max(endFullSpeed, 0);
+    double endDeccel = m_endDeccel - endAccel - endFullSpeed;
+    endDeccel = Math.max(endDeccel, 0);
+
+    final double acceleration = m_constraints.maxAcceleration;
+    final double decceleration = -m_constraints.maxAcceleration;
+
+    double distToTarget = Math.abs(target - position);
+    if (distToTarget < 1e-6) {
+      return 0;
+    }
+
+    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+    double deccelVelocity;
+    if (endAccel > 0) {
+      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
+    } else {
+      deccelVelocity = velocity;
+    }
+
+    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+    deccelDist = Math.max(deccelDist, 0);
+
+    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+    if (accelDist > distToTarget) {
+      accelDist = distToTarget;
+      fullSpeedDist = 0;
+      deccelDist = 0;
+    } else if (accelDist + fullSpeedDist > distToTarget) {
+      fullSpeedDist = distToTarget - accelDist;
+      deccelDist = 0;
+    } else {
+      deccelDist = distToTarget - fullSpeedDist - accelDist;
+    }
+
+    double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
+        * accelDist))) / acceleration;
+
+    double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
+        + 2 * decceleration * deccelDist))) / decceleration;
+
+    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+    return accelTime + fullSpeedTime + deccelTime;
+  }
+
+  /**
+   * Returns the total time the profile takes to reach the goal.
+   */
+  public double totalTime() {
+    return m_endDeccel;
+  }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * <p>The profile has reached the goal if the time since the profile started
+   * has exceeded the profile's total time.
+   *
+   * @param t The time since the beginning of the profile.
+   */
+  @SuppressWarnings("ParameterName")
+  public boolean isFinished(double t) {
+    return t >= totalTime();
+  }
+
+  /**
+   * Returns true if the profile inverted.
+   *
+   * <p>The profile is inverted if goal position is less than the initial position.
+   *
+   * @param initial     The initial state (usually the current state).
+   * @param goal        The desired state when the profile is complete.
+   */
+  @SuppressWarnings("LocalVariableName")
+  private static boolean shouldFlipAcceleration(State initial, State goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  private State direct(State in) {
+    State result = new State(in.position, in.velocity);
+    result.position = result.position * m_direction;
+    result.velocity = result.velocity * m_direction;
+    return result;
+  }
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TrapezoidProfileTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TrapezoidProfileTest.java
new file mode 100644
index 0000000000..77b099e6e1
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TrapezoidProfileTest.java
@@ -0,0 +1,259 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+@SuppressWarnings({"PMD.TooManyMethods", "PMD.AvoidInstantiatingObjectsInLoops"})
+class TrapezoidProfileTest {
+  private static final double kDt = 0.01;
+
+  /**
+   * Asserts "val1" is less than or equal to "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   */
+  private static void assertLessThanOrEquals(double val1, double val2) {
+    assertTrue(val1 <= val2, Double.toString(val1) + " is greater than " + val2);
+  }
+
+  /**
+   * Asserts "val1" is within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertNear(double val1, double val2, double eps) {
+    assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
+        + " is greater than " + eps);
+  }
+
+  /**
+   * Asserts "val1" is less than or within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertLessThanOrNear(double val1, double val2, double eps) {
+    if (val1 <= val2) {
+      assertLessThanOrEquals(val1, val2);
+    } else {
+      assertNear(val1, val2, eps);
+    }
+  }
+
+  @Test
+  void reachesGoal() {
+    TrapezoidProfile.Constraints constraints =
+        new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 450; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Tests that decreasing the maximum velocity in the middle when it is already
+  // moving faster than the new max is handled correctly
+  @Test
+  void posContinousUnderVelChange() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double lastPos = state.position;
+    for (int i = 0; i < 1600; ++i) {
+      if (i == 400) {
+        constraints.maxVelocity = 0.75;
+      }
+
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      double estimatedVel = (state.position - lastPos) / kDt;
+
+      if (i >= 400) {
+        // Since estimatedVel can have floating point rounding errors, we check
+        // whether value is less than or within an error delta of the new
+        // constraint.
+        assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
+
+        assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
+      }
+
+      lastPos = state.position;
+    }
+    assertEquals(state, goal);
+  }
+
+  // There is some somewhat tricky code for dealing with going backwards
+  @Test
+  void backwards() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void switchGoalInMiddle() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNotEquals(state, goal);
+
+    goal = new TrapezoidProfile.State(0.0, 0.0);
+    for (int i = 0; i < 550; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Checks to make sure that it hits top speed
+  @Test
+  void topSpeed() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNear(constraints.maxVelocity, state.velocity, 10e-5);
+
+    for (int i = 0; i < 2000; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void timingToCurrent() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; i++) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
+    }
+  }
+
+  @Test
+  void timingToGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(1);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingToNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(goal)) {
+        // Expected value using for loop index is just an approximation since
+        // the time left in the profile doesn't increase linearly at the
+        // endpoints
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(-1);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+}