Add RamseteCommand (#1951)

wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp

@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc2/command/RamseteCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+template <typename T>
+int sgn(T val) {
+  return (T(0) < val) - (val < T(0));
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller, volt_t ks,
+    units::unit_t<voltsecondspermeter> kv,
+    units::unit_t<voltsecondssquaredpermeter> ka,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<units::meters_per_second_t()> leftSpeed,
+    std::function<units::meters_per_second_t()> rightSpeed,
+    frc2::PIDController leftController, frc2::PIDController rightController,
+    std::function<void(volt_t, volt_t)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(ks),
+      m_kv(kv),
+      m_ka(ka),
+      m_kinematics(kinematics),
+      m_leftSpeed(leftSpeed),
+      m_rightSpeed(rightSpeed),
+      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+      m_outputVolts(output) {
+  AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::RamseteController controller,
+    frc::DifferentialDriveKinematics kinematics,
+    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+        output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(trajectory),
+      m_pose(pose),
+      m_controller(controller),
+      m_ks(0),
+      m_kv(0),
+      m_ka(0),
+      m_kinematics(kinematics),
+      m_outputVel(output) {
+  AddRequirements(requirements);
+}
+
+void RamseteCommand::Initialize() {
+  m_prevTime = 0_s;
+  auto initialState = m_trajectory.Sample(0_s);
+  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+      frc::ChassisSpeeds{initialState.velocity, 0_mps,
+                         initialState.velocity * initialState.curvature});
+  m_timer.Reset();
+  m_timer.Start();
+  m_leftController->Reset();
+  m_rightController->Reset();
+}
+
+void RamseteCommand::Execute() {
+  auto curTime = m_timer.Get();
+  auto dt = curTime - m_prevTime;
+
+  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
+      m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
+
+  if (m_leftController.get() != nullptr) {
+    auto leftFeedforward =
+        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
+        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+
+    auto rightFeedforward =
+        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
+        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+
+    auto leftOutput =
+        volt_t(m_leftController->Calculate(
+            m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
+        leftFeedforward;
+
+    auto rightOutput = volt_t(m_rightController->Calculate(
+                           m_rightSpeed().to<double>(),
+                           targetWheelSpeeds.right.to<double>())) +
+                       rightFeedforward;
+
+    m_outputVolts(leftOutput, rightOutput);
+  } else {
+    m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
+  }
+
+  m_prevTime = curTime;
+  m_prevSpeeds = targetWheelSpeeds;
+}
+
+void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool RamseteCommand::IsFinished() {
+  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}

wpilibc/src/main/native/include/frc2/command/RamseteCommand.h

@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <functional>
+#include <memory>
+
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that uses a RAMSETE controller  to follow a trajectory
+ * with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and
+ * feedforwards internally.  This is intended to be a more-or-less "complete
+ * solution" that can be used by teams without a great deal of controls
+ * expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw wheel speeds from the RAMSETE controller.
+ *
+ * @see RamseteController
+ * @see Trajectory
+ */
+class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
+  using voltsecondspermeter =
+      units::compound_unit<units::volt, units::second,
+                           units::inverse<units::meter>>;
+  using voltsecondssquaredpermeter =
+      units::compound_unit<units::volt, units::squared<units::second>,
+                           units::inverse<units::meter>>;
+
+ public:
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. PID control and feedforward are handled internally,
+   * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
+   *
+   * <p>Note: The controller will *not* set the outputVolts to zero upon
+   * completion of the path - this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param ks              Constant feedforward term for the robot drive.
+   * @param kv              Velocity-proportional feedforward term for the robot
+   * drive.
+   * @param ka              Acceleration-proportional feedforward term for the
+   * robot drive.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param leftSpeed       A function that supplies the speed of the left side
+   * of the robot drive.
+   * @param rightSpeed      A function that supplies the speed of the right side
+   * of the robot drive.
+   * @param leftController  The PIDController for the left side of the robot
+   * drive.
+   * @param rightController The PIDController for the right side of the robot
+   * drive.
+   * @param output          A function that consumes the computed left and right
+   * outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller, units::volt_t ks,
+                 units::unit_t<voltsecondspermeter> kv,
+                 units::unit_t<voltsecondssquaredpermeter> ka,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<units::meters_per_second_t()> leftSpeed,
+                 std::function<units::meters_per_second_t()> rightSpeed,
+                 frc2::PIDController leftController,
+                 frc2::PIDController rightController,
+                 std::function<void(units::volt_t, units::volt_t)> output,
+                 std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the
+   * provided trajectory. Performs no PID control and calculates no
+   * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+   * and will need to be converted into a usable form by the user.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose - use one of
+   * the odometry classes to provide this.
+   * @param controller      The RAMSETE controller used to follow the
+   * trajectory.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param output          A function that consumes the computed left and right
+   * outputs (in volts) for the robot drive.
+   * @param requirements    The subsystems to require.
+   */
+  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+                 frc::RamseteController controller,
+                 frc::DifferentialDriveKinematics kinematics,
+                 std::function<void(units::meters_per_second_t,
+                                    units::meters_per_second_t)>
+                     output,
+                 std::initializer_list<Subsystem*> requirements);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  frc::Trajectory m_trajectory;
+  std::function<frc::Pose2d()> m_pose;
+  frc::RamseteController m_controller;
+  const units::volt_t m_ks;
+  const units::unit_t<voltsecondspermeter> m_kv;
+  const units::unit_t<voltsecondssquaredpermeter> m_ka;
+  frc::DifferentialDriveKinematics m_kinematics;
+  std::function<units::meters_per_second_t()> m_leftSpeed;
+  std::function<units::meters_per_second_t()> m_rightSpeed;
+  std::unique_ptr<frc2::PIDController> m_leftController;
+  std::unique_ptr<frc2::PIDController> m_rightController;
+  std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
+  std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+      m_outputVel;
+
+  Timer m_timer;
+  units::second_t m_prevTime;
+  frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
+};
+}  // namespace frc2

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp

@@ -47,7 +47,7 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) {
 }
 
 double DriveSubsystem::GetHeading() {
-  return std::remainder(m_gyro.GetAngle(), 360);
+  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
 }
 
 double DriveSubsystem::GetTurnRate() {

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h

@@ -33,7 +33,7 @@ const double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
     (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
 
-const bool kGyroReversed = false;
+const bool kGyroReversed = true;
 
 const double kStabilizationP = 1;
 const double kStabilizationI = .5;

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "Robot.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp

@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "RobotContainer.h"
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/RamseteCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+RobotContainer::RobotContainer() {
+  // Initialize all of your commands and subsystems here
+
+  // Configure the button bindings
+  ConfigureButtonBindings();
+
+  // Set up default drive command
+  m_drive.SetDefaultCommand(frc2::RunCommand(
+      [this] {
+        m_drive.ArcadeDrive(
+            m_driverController.GetY(frc::GenericHID::kLeftHand),
+            m_driverController.GetX(frc::GenericHID::kRightHand));
+      },
+      {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Configure your button bindings here
+
+  // While holding the shoulder button, drive at half speed
+  frc2::JoystickButton(&m_driverController, 6)
+      .WhenPressed(&m_driveHalfSpeed)
+      .WhenReleased(&m_driveFullSpeed);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Set up config for trajectory
+  frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
+                               AutoConstants::kMaxAcceleration);
+  // Add kinematics to ensure max speed is actually obeyed
+  config.SetKinematics(DriveConstants::kDriveKinematics);
+
+  // An example trajectory to follow.  All units in meters.
+  auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      // Start at the origin facing the +X direction
+      frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
+      // Pass through these two interior waypoints, making an 's' curve path
+      {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+      // End 3 meters straight ahead of where we started, facing forward
+      frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
+      // Pass the config
+      config);
+
+  frc2::RamseteCommand ramseteCommand(
+      exampleTrajectory, [this]() { return m_drive.GetPose(); },
+      frc::RamseteController(AutoConstants::kRamseteB,
+                             AutoConstants::kRamseteZeta),
+      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka,
+      DriveConstants::kDriveKinematics,
+      [this] {
+        return units::meters_per_second_t(m_drive.GetLeftEncoder().GetRate());
+      },
+      [this] {
+        return units::meters_per_second_t(m_drive.GetRightEncoder().GetRate());
+      },
+      frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+      frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
+      [this](auto left, auto right) {
+        m_drive.TankDrive(left / 12_V, right / 12_V);
+      },
+      {&m_drive});
+
+  // no auto
+  return new frc2::SequentialCommandGroup(
+      std::move(ramseteCommand),
+      frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
+}

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "subsystems/DriveSubsystem.h"
+
+#include <units/units.h>
+
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+    : m_left1{kLeftMotor1Port},
+      m_left2{kLeftMotor2Port},
+      m_right1{kRightMotor1Port},
+      m_right2{kRightMotor2Port},
+      m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
+      m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
+      m_odometry{kDriveKinematics, frc::Pose2d()} {
+  // Set the distance per pulse for the encoders
+  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+  m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
+                    frc::DifferentialDriveWheelSpeeds{
+                        units::meters_per_second_t(m_leftEncoder.GetRate()),
+                        units::meters_per_second_t(m_rightEncoder.GetRate())});
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::TankDrive(double left, double right) {
+  m_drive.TankDrive(left, right, false);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+double DriveSubsystem::GetAverageEncoderDistance() {
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}
+
+double DriveSubsystem::GetHeading() {
+  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+double DriveSubsystem::GetTurnRate() {
+  return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+}
+
+frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+
+void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
+  m_odometry.ResetPosition(pose);
+}

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/units.h>
+
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+const int kLeftMotor1Port = 0;
+const int kLeftMotor2Port = 1;
+const int kRightMotor1Port = 2;
+const int kRightMotor2Port = 3;
+
+const int kLeftEncoderPorts[]{0, 1};
+const int kRightEncoderPorts[]{2, 3};
+const bool kLeftEncoderReversed = false;
+const bool kRightEncoderReversed = true;
+
+const auto kTrackwidth = .6_m;
+const frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
+
+const int kEncoderCPR = 1024;
+const double kWheelDiameterInches = 6;
+const double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+
+const bool kGyroReversed = true;
+
+// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+// These characterization values MUST be determined either experimentally or
+// theoretically for *your* robot's drive. The RobotPy Characterization
+// Toolsuite provides a convenient tool for obtaining these values for your
+// robot.
+const auto ks = 1_V;
+const auto kv = .8 * 1_V * 1_s / 1_m;
+const auto ka = .15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+const double kPDriveVel = .5;
+}  // namespace DriveConstants
+
+namespace AutoConstants {
+const auto kMaxSpeed = 3_mps;
+const auto kMaxAcceleration = 3_mps_sq;
+
+// Reasonable baseline values for a RAMSETE follower in units of meters and
+// seconds
+const double kRamseteB = 2;
+const double kRamseteZeta = .7;
+}  // namespace AutoConstants
+
+namespace OIConstants {
+const int kDriverControllerPort = 1;
+}  // namespace OIConstants

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+
+  RobotContainer m_container;
+};

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+ public:
+  RobotContainer();
+
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // The driver's controller
+  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+
+  // The robot's subsystems and commands are defined here...
+
+  // The robot's subsystems
+  DriveSubsystem m_drive;
+
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+                                        {}};
+  frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+                                        {}};
+
+  // The chooser for the autonomous routines
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h

@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/ADXRS450_Gyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  // Subsystem methods go here.
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  void ArcadeDrive(double fwd, double rot);
+
+  /**
+   * Drives the robot using tank controls.  Does not square inputs to enable
+   * composition with external controllers.
+   *
+   * @param left the commanded left output
+   * @param right the commanded right output
+   */
+  void TankDrive(double left, double right);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  double GetAverageEncoderDistance();
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  frc::Encoder& GetLeftEncoder();
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  frc::Encoder& GetRightEncoder();
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive
+   * more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  void SetMaxOutput(double maxOutput);
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from 180 to 180
+   */
+  double GetHeading();
+
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  double GetTurnRate();
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   *
+   * @return The pose.
+   */
+  frc::Pose2d GetPose();
+
+  /**
+   * Resets the odometry to the specified pose.
+   *
+   * @param pose The pose to which to set the odometry.
+   */
+  void ResetOdometry(frc::Pose2d pose);
+
+ private:
+  // Components (e.g. motor controllers and sensors) should generally be
+  // declared private and exposed only through public methods.
+
+  // The motor controllers
+  frc::PWMVictorSPX m_left1;
+  frc::PWMVictorSPX m_left2;
+  frc::PWMVictorSPX m_right1;
+  frc::PWMVictorSPX m_right2;
+
+  // The motors on the left side of the drive
+  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+
+  // The motors on the right side of the drive
+  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+
+  // The robot's drive
+  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+  // The left-side drive encoder
+  frc::Encoder m_leftEncoder;
+
+  // The right-side drive encoder
+  frc::Encoder m_rightEncoder;
+
+  // The gyro sensor
+  frc::ADXRS450_Gyro m_gyro;
+
+  // Odometry class for tracking robot pose
+  frc::DifferentialDriveOdometry m_odometry;
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -355,5 +355,18 @@
     ],
     "foldername": "DifferentialDriveBot",
     "gradlebase": "cpp"
+  },
+  {
+    "name:": "RamseteCommand",
+    "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
+    "tags": [
+      "RamseteCommand",
+      "PID",
+      "Ramsete",
+      "Trajectory",
+      "Path following"
+    ],
+    "foldername": "RamseteCommand",
+    "gradlebase": "cpp"
   }
 ]

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.BiConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
+ * {@link Trajectory} with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and feedforwards internally.  This
+ * is intended to be a more-or-less "complete solution" that can be used by teams without a great
+ * deal of controls expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
+ * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
+ * the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE
+ * controller.
+ */
+public class RamseteCommand extends CommandBase {
+  private final Timer m_timer = new Timer();
+  private DifferentialDriveWheelSpeeds m_prevSpeeds;
+  private double m_prevTime;
+
+  private final Trajectory m_trajectory;
+  private final Supplier<Pose2d> m_pose;
+  private final RamseteController m_follower;
+  private final double m_ks;
+  private final double m_kv;
+  private final double m_ka;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final DoubleSupplier m_leftSpeed;
+  private final DoubleSupplier m_rightSpeed;
+  private final PIDController m_leftController;
+  private final PIDController m_rightController;
+  private final BiConsumer<Double, Double> m_output;
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+   * PID control and feedforward are handled internally, and outputs are scaled -1 to 1 for easy
+   * consumption by speed controllers.
+   *
+   * <p>Note: The controller will *not* set the outputVolts to zero upon completion of the path -
+   * this
+   * is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory                     The trajectory to follow.
+   * @param pose                           A function that supplies the robot pose - use one of
+   *                                       the odometry classes to provide this.
+   * @param controller                       The RAMSETE controller used to follow the trajectory.
+   * @param ksVolts                        Constant feedforward term for the robot drive.
+   * @param kvVoltSecondsPerMeter          Velocity-proportional feedforward term for the robot
+   *                                       drive.
+   * @param kaVoltSecondsSquaredPerMeter   Acceleration-proportional feedforward term for the robot
+   *                                       drive.
+   * @param kinematics                     The kinematics for the robot drivetrain.
+   * @param leftWheelSpeedMetersPerSecond  A function that supplies the speed of the left side of
+   *                                       the robot drive.
+   * @param rightWheelSpeedMetersPerSecond A function that supplies the speed of the right side of
+   *                                       the robot drive.
+   * @param leftController                 The PIDController for the left side of the robot drive.
+   * @param rightController                The PIDController for the right side of the robot drive.
+   * @param outputVolts                    A function that consumes the computed left and right
+   *                                       outputs (in volts) for the robot drive.
+   * @param requirements                   The subsystems to require.
+   */
+  @SuppressWarnings("PMD.ExcessiveParameterList")
+  public RamseteCommand(Trajectory trajectory,
+                        Supplier<Pose2d> pose,
+                        RamseteController controller,
+                        double ksVolts,
+                        double kvVoltSecondsPerMeter,
+                        double kaVoltSecondsSquaredPerMeter,
+                        DifferentialDriveKinematics kinematics,
+                        DoubleSupplier leftWheelSpeedMetersPerSecond,
+                        DoubleSupplier rightWheelSpeedMetersPerSecond,
+                        PIDController leftController,
+                        PIDController rightController,
+                        BiConsumer<Double, Double> outputVolts,
+                        Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
+    m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
+    m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
+    m_ks = ksVolts;
+    m_kv = kvVoltSecondsPerMeter;
+    m_ka = kaVoltSecondsSquaredPerMeter;
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+    m_leftSpeed = requireNonNullParam(leftWheelSpeedMetersPerSecond,
+                                      "leftWheelSpeedMetersPerSecond",
+                                      "RamseteCommand");
+    m_rightSpeed = requireNonNullParam(rightWheelSpeedMetersPerSecond,
+                                       "rightWheelSpeedMetersPerSecond",
+                                       "RamseteCommand");
+    m_leftController = requireNonNullParam(leftController, "leftController", "RamseteCommand");
+    m_rightController = requireNonNullParam(rightController, "rightController", "RamseteCommand");
+    m_output = requireNonNullParam(outputVolts, "outputVolts", "RamseteCommand");
+
+    addRequirements(requirements);
+  }
+
+  /**
+   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
+   * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds
+   * from the RAMSETE controller, and will need to be converted into a usable form by the user.
+   *
+   * @param trajectory            The trajectory to follow.
+   * @param pose                  A function that supplies the robot pose - use one of
+   *                              the odometry classes to provide this.
+   * @param follower              The RAMSETE follower used to follow the trajectory.
+   * @param kinematics            The kinematics for the robot drivetrain.
+   * @param outputMetersPerSecond A function that consumes the computed left and right
+   *                              wheel speeds.
+   * @param requirements          The subsystems to require.
+   */
+  public RamseteCommand(Trajectory trajectory,
+                        Supplier<Pose2d> pose,
+                        RamseteController follower,
+                        DifferentialDriveKinematics kinematics,
+                        BiConsumer<Double, Double> outputMetersPerSecond,
+                        Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
+    m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
+    m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "RamseteCommand");
+    m_output = requireNonNullParam(outputMetersPerSecond, "output", "RamseteCommand");
+
+    m_ks = 0;
+    m_kv = 0;
+    m_ka = 0;
+    m_leftSpeed = null;
+    m_rightSpeed = null;
+    m_leftController = null;
+    m_rightController = null;
+
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_prevTime = 0;
+    var initialState = m_trajectory.sample(0);
+    m_prevSpeeds = m_kinematics.toWheelSpeeds(
+        new ChassisSpeeds(initialState.velocityMetersPerSecond,
+                          0,
+                          initialState.curvatureRadPerMeter
+                              * initialState.velocityMetersPerSecond));
+    m_timer.reset();
+    m_timer.start();
+    m_leftController.reset();
+    m_rightController.reset();
+  }
+
+  @Override
+  public void execute() {
+    double curTime = m_timer.get();
+    double dt = curTime - m_prevTime;
+
+    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
+        m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
+
+    var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
+    var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
+
+    double leftOutput;
+    double rightOutput;
+
+    if (m_leftController != null) {
+      double leftFeedforward =
+          m_ks * Math.signum(leftSpeedSetpoint)
+              + m_kv * leftSpeedSetpoint
+              + m_ka * (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt;
+
+      double rightFeedforward =
+          m_ks * Math.signum(rightSpeedSetpoint)
+              + m_kv * rightSpeedSetpoint
+              + m_ka * (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt;
+
+      leftOutput = leftFeedforward
+          + m_leftController.calculate(m_leftSpeed.getAsDouble(),
+                                       leftSpeedSetpoint);
+
+      rightOutput = rightFeedforward
+          + m_rightController.calculate(m_rightSpeed.getAsDouble(),
+                                        rightSpeedSetpoint);
+    } else {
+      leftOutput = leftSpeedSetpoint;
+      rightOutput = rightSpeedSetpoint;
+    }
+
+    m_output.accept(leftOutput, rightOutput);
+
+    m_prevTime = curTime;
+    m_prevSpeeds = targetWheelSpeeds;
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds());
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -379,5 +379,19 @@
     "foldername": "differentialdrivebot",
     "gradlebase": "java",
     "mainclass": "Main"
+  },
+  {
+    "name:": "RamseteCommand",
+    "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
+    "tags": [
+      "RamseteCommand",
+      "PID",
+      "Ramsete",
+      "Trajectory",
+      "Path following"
+    ],
+    "foldername": "ramsetecommand",
+    "gradlebase": "java",
+    "mainclass": "Main"
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.ramsetecommand;
+
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
+    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final double kTrackwidthMeters = .6;
+    public static final DifferentialDriveKinematics kDriveKinematics =
+        new DifferentialDriveKinematics(kTrackwidthMeters);
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterMeters = .15;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+
+    public static final boolean kGyroReversed = true;
+
+    // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
+    // These characterization values MUST be determined either experimentally or theoretically
+    // for *your* robot's drive.
+    // The RobotPy Characterization Toolsuite provides a convenient tool for obtaining these
+    // values for your robot.
+    public static final double ksVolts = 1;
+    public static final double kvVoltSecondsPerMeter = .8;
+    public static final double kaVoltSecondsSquaredPerMeter = .15;
+
+    // Example value only - as above, this must be tuned for your drive!
+    public static final double kPDriveVel = .5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 1;
+  }
+
+  public static final class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+
+    public static final DifferentialDriveKinematicsConstraint kAutoPathConstraints =
+        new DifferentialDriveKinematicsConstraint(DriveConstants.kDriveKinematics,
+                                                  kMaxSpeedMetersPerSecond);
+
+    // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
+    public static final double kRamseteB = 2;
+    public static final double kRamseteZeta = .7;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.examples.ramsetecommand;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java

@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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.examples.ramsetecommand;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ramsetecommand;
+
+import java.util.List;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteB;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteZeta;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kPDriveVel;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.ksVolts;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kvVoltSecondsPerMeter;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants.kDriverControllerPort;
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+
+  // The driver's controller
+  XboxController m_driverController = new XboxController(kDriverControllerPort);
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    m_robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        new RunCommand(() -> m_robotDrive
+            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
+                         m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
+   * {@link JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Drive at half speed when the right bumper is held
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenPressed(() -> m_robotDrive.setMaxOutput(.5))
+        .whenReleased(() -> m_robotDrive.setMaxOutput(1));
+
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // Create config for trajectory
+    TrajectoryConfig config =
+        new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared)
+            // Add kinematics to ensure max speed is actually obeyed
+            .setKinematics(kDriveKinematics);
+
+    // An example trajectory to follow.  All units in meters.
+    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+        // Start at the origin facing the +X direction
+        new Pose2d(0, 0, new Rotation2d(0)),
+        // Pass through these two interior waypoints, making an 's' curve path
+        List.of(
+            new Translation2d(1, 1),
+            new Translation2d(2, -1)
+        ),
+        // End 3 meters straight ahead of where we started, facing forward
+        new Pose2d(3, 0, new Rotation2d(0)),
+        // Pass config
+        config
+    );
+
+    RamseteCommand ramseteCommand = new RamseteCommand(
+        exampleTrajectory,
+        m_robotDrive::getPose,
+        new RamseteController(kRamseteB, kRamseteZeta),
+        ksVolts,
+        kvVoltSecondsPerMeter,
+        kaVoltSecondsSquaredPerMeter,
+        kDriveKinematics,
+        m_robotDrive.getLeftEncoder()::getRate,
+        m_robotDrive.getRightEncoder()::getRate,
+        new PIDController(kPDriveVel, 0, 0),
+        new PIDController(kPDriveVel, 0, 0),
+        // RamseteCommand passes volts to the callback, so we have to rescale here
+        (left, right) -> m_robotDrive.tankDrive(left / 12., right / 12.),
+        m_robotDrive
+    );
+
+    // Run path following command, then stop at the end.
+    return ramseteCommand.andThen(() -> m_robotDrive.tankDrive(0, 0));
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java

@@ -0,0 +1,188 @@
+/*----------------------------------------------------------------------------*/
+/* 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.examples.ramsetecommand.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor2Port;
+
+public class DriveSubsystem extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final SpeedControllerGroup m_leftMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port),
+          new PWMVictorSPX(kLeftMotor2Port));
+
+  // The motors on the right side of the drive.
+  private final SpeedControllerGroup m_rightMotors =
+      new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port),
+          new PWMVictorSPX(kRightMotor2Port));
+
+  // The robot's drive
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed);
+
+  // The gyro sensor
+  private final Gyro m_gyro = new ADXRS450_Gyro();
+
+  // Odometry class for tracking robot pose
+  DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics);
+
+  /**
+   * Creates a new DriveSubsystem.
+   */
+  public DriveSubsystem() {
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+  }
+
+  @Override
+  public void periodic() {
+    // Update the odometry in the periodic block
+    m_odometry.update(new Rotation2d(getHeading()),
+                                  new DifferentialDriveWheelSpeeds(
+                                      m_leftEncoder.getRate(),
+                                      m_rightEncoder.getRate()
+                                  ));
+  }
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   *
+   * @return The pose.
+   */
+  public Pose2d getPose() {
+    return m_odometry.getPoseMeters();
+  }
+
+  /**
+   * Resets the odometry to the specified pose.
+   *
+   * @param pose The pose to which to set the odometry.
+   */
+  public void resetOdometry(Pose2d pose) {
+    m_odometry.resetPosition(pose);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    m_drive.arcadeDrive(fwd, rot);
+  }
+
+  /**
+   * Drives the robot using tank controls.  Does not square inputs to enable composition with
+   * external controllers.
+   *
+   * @param left the commanded left output
+   * @param right the commanded right output
+   */
+  public void tankDrive(double left, double right) {
+    m_drive.tankDrive(left, right, false);
+  }
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the two encoders.
+   *
+   * @return the average of the two encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.;
+  }
+
+  /**
+   * Gets the left drive encoder.
+   *
+   * @return the left drive encoder
+   */
+  public Encoder getLeftEncoder() {
+    return m_leftEncoder;
+  }
+
+  /**
+   * Gets the right drive encoder.
+   *
+   * @return the right drive encoder
+   */
+  public Encoder getRightEncoder() {
+    return m_rightEncoder;
+  }
+
+  /**
+   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  public void setMaxOutput(double maxOutput) {
+    m_drive.setMaxOutput(maxOutput);
+  }
+
+  /**
+   * Zeroes the heading of the robot.
+   */
+  public void zeroHeading() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from 180 to 180
+   */
+  public double getHeading() {
+    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.);
+  }
+
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  public double getTurnRate() {
+    return m_gyro.getRate() * (kGyroReversed ? -1. : 1.);
+  }
+}

wpiutil/src/main/native/include/units/units.h

@@ -4853,4 +4853,5 @@ using namespace time;
 using namespace velocity;
 using namespace acceleration;
 using namespace angle;
+using namespace voltage;
 }  // namespace units