Add ProfiledPIDSubsystem example (#2076)

wpilibcExamples/src/main/cpp/examples/ArmBot/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/ArmBot/cpp/RobotContainer.cpp

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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/shuffleboard/Shuffleboard.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.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
+
+  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
+
+  // Move the arm to neutral position when the 'B' button is pressed.
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
+                   {&m_arm});
+
+  // While holding the shoulder button, drive at half speed
+  frc2::JoystickButton(&m_driverController, 6)
+      .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+      .WhenReleased([this] { m_drive.SetMaxOutput(1); });
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Runs the chosen command in autonomous
+  return new frc2::InstantCommand([] {});
+}

wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ArmSubsystem.h"
+
+#include "Constants.h"
+
+using namespace ArmConstants;
+using State = frc::TrapezoidProfile<units::radians>::State;
+
+ArmSubsystem::ArmSubsystem()
+    : frc2::ProfiledPIDSubsystem<units::radians>(
+          frc::ProfiledPIDController<units::radians>(
+              kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
+      m_motor(kMotorPort),
+      m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
+      m_feedforward(kS, kCos, kV, kA),
+      // Start arm at rest in neutral position
+      m_goal{kArmOffset, 0_rad_per_s} {
+  m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+}
+
+void ArmSubsystem::UseOutput(double output, State setpoint) {
+  // Calculate the feedforward from the sepoint
+  units::volt_t feedforward =
+      m_feedforward.Calculate(setpoint.position, setpoint.velocity);
+  // Add the feedforward to the PID output to get the motor output
+  m_motor.SetVoltage(units::volt_t(output) + feedforward);
+}
+
+void ArmSubsystem::SetGoal(units::radian_t goal) {
+  m_goal = State{goal, 0_rad_per_s};
+}
+
+State ArmSubsystem::GetGoal() { return m_goal; }
+
+units::radian_t ArmSubsystem::GetMeasurement() {
+  return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
+}

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

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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"
+
+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]} {
+  // 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.
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+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);
+}

wpilibcExamples/src/main/cpp/examples/ArmBot/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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+/**
+ * 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 {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+}  // namespace DriveConstants
+
+namespace ArmConstants {
+constexpr int kMotorPort = 4;
+
+constexpr double kP = 1;
+
+// These are fake gains; in actuality these must be determined individually for
+// each robot
+constexpr auto kS = 1_V;
+constexpr auto kCos = 1_V;
+constexpr auto kV = 0.5_V * 1_s / 1_rad;
+constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad;
+
+constexpr auto kMaxVelocity = 3_rad_per_s;
+constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s);
+
+constexpr int kEncoderPorts[]{4, 5};
+constexpr int kEncoderPPR = 256;
+constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+
+// The offset of the arm from the horizontal in its neutral position,
+// measured from the horizontal
+constexpr auto kArmOffset = 0.5_rad;
+}  // namespace ArmConstants
+
+namespace AutoConstants {
+constexpr auto kAutoTimeoutSeconds = 12_s;
+constexpr auto kAutoShootTimeSeconds = 7_s;
+}  // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants

wpilibcExamples/src/main/cpp/examples/ArmBot/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/ArmBot/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/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/ConditionalCommand.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/WaitCommand.h>
+#include <frc2/command/WaitUntilCommand.h>
+
+#include "Constants.h"
+#include "subsystems/ArmSubsystem.h"
+#include "subsystems/DriveSubsystem.h"
+
+namespace ac = AutoConstants;
+
+/**
+ * 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;
+  ArmSubsystem m_arm;
+
+  // The chooser for the autonomous routines
+
+  void ConfigureButtonBindings();
+};

wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/controller/ArmFeedforward.h>
+#include <frc2/command/ProfiledPIDSubsystem.h>
+#include <units/units.h>
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
+  using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+  ArmSubsystem();
+
+  void UseOutput(double output, State setpoint) override;
+
+  void SetGoal(units::radian_t goal);
+
+  State GetGoal() override;
+
+  units::radian_t GetMeasurement() override;
+
+ private:
+  frc::PWMVictorSPX m_motor;
+  frc::Encoder m_encoder;
+  frc::ArmFeedforward m_feedforward;
+
+  State m_goal;
+};

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

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/drive/DifferentialDrive.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);
+
+  /**
+   * 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);
+
+ 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;
+};

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

@@ -496,5 +496,17 @@
     "foldername": "SwerveControllerCommand",
     "gradlebase": "cpp",
     "commandversion": 2
+  },
+  {
+    "name": "ArmBot",
+    "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
+    "tags": [
+      "ArmBot",
+      "PID",
+      "Motion Profile"
+    ],
+    "foldername": "ArmBot",
+    "gradlebase": "cpp",
+    "commandversion": 2
   }
 ]

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

@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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.armbot;
+
+/**
+ * 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 int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class ArmConstants {
+    public static final int kMotorPort = 4;
+
+    public static final double kP = 1;
+
+    // These are fake gains; in actuality these must be determined individually for each robot
+    public static final double kSVolts = 1;
+    public static final double kCosVolts = 1;
+    public static final double kVVoltSecondPerRad = 0.5;
+    public static final double kAVoltSecondSquaredPerRad = 0.1;
+
+    public static final double kMaxVelocityRadPerSecond = 3;
+    public static final double kMaxAccelerationRadPerSecSquared = 10;
+
+    public static final int[] kEncoderPorts = new int[]{4, 5};
+    public static final int kEncoderPPR = 256;
+    public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
+
+    // The offset of the arm from the horizontal in its neutral position,
+    // measured from the horizontal
+    public static final double kArmOffsetRads = 0.5;
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoTimeoutSeconds = 12;
+    public static final double kAutoShootTimeSeconds = 7;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 1;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/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.armbot;
+
+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/armbot/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.armbot;
+
+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/armbot/RobotContainer.java

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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.armbot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
+
+import static edu.wpi.first.wpilibj.XboxController.Button;
+import static edu.wpi.first.wpilibj.examples.armbot.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();
+  private final ArmSubsystem m_robotArm = new ArmSubsystem();
+
+  // 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 passing it to a
+   * {@link JoystickButton}.
+   */
+  private void configureButtonBindings() {
+
+    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
+    new JoystickButton(m_driverController, Button.kA.value)
+        .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
+
+    // Move the arm to neutral position when the 'B' button is pressed.
+    new JoystickButton(m_driverController, Button.kB.value)
+        .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
+
+    // Drive at half speed when the bumper is held
+    new JoystickButton(m_driverController, Button.kBumperRight.value)
+        .whenPressed(() -> m_robotDrive.setMaxOutput(0.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() {
+    return new InstantCommand();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java

@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* 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.armbot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PWMVictorSPX;
+import edu.wpi.first.wpilibj.controller.ArmFeedforward;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
+
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kAVoltSecondSquaredPerRad;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kArmOffsetRads;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kCosVolts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxVelocityRadPerSecond;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMotorPort;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kP;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kSVolts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kVVoltSecondPerRad;
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+public class ArmSubsystem extends ProfiledPIDSubsystem {
+  private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+  private final Encoder m_encoder = new Encoder(kEncoderPorts[0], kEncoderPorts[1]);
+  private final ArmFeedforward m_feedforward =
+      new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad);
+
+  private TrapezoidProfile.State m_goal;
+
+  /**
+   * Create a new ArmSubsystem.
+   */
+  public ArmSubsystem() {
+    super(new ProfiledPIDController(
+        kP,
+        0,
+        0,
+        new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
+                                         kMaxAccelerationRadPerSecSquared)));
+    m_encoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    // Start arm at rest in neutral position
+    m_goal = new TrapezoidProfile.State(kArmOffsetRads, 0);
+  }
+
+  @Override
+  public void useOutput(double output, TrapezoidProfile.State setpoint) {
+    // Calculate the feedforward from the sepoint
+    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
+    // Add the feedforward to the PID output to get the motor output
+    m_motor.setVoltage(output + feedforward);
+  }
+
+  /**
+   * Sets the goal position for the arm.
+   *
+   * @param position The goal position, in radians.
+   */
+  public void setGoal(double position) {
+    m_goal = new TrapezoidProfile.State(position, 0);
+  }
+
+  @Override
+  public TrapezoidProfile.State getGoal() {
+    return m_goal;
+  }
+
+  @Override
+  public double getMeasurement() {
+    return m_encoder.getDistance() + kArmOffsetRads;
+  }
+}

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

@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* 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.armbot.subsystems;
+
+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.wpilibj2.command.SubsystemBase;
+
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kEncoderDistancePerPulse;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor2Port;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderPorts;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderReversed;
+import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor1Port;
+import static edu.wpi.first.wpilibj.examples.armbot.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);
+
+  /**
+   * Creates a new DriveSubsystem.
+   */
+  public DriveSubsystem() {
+    // Sets the distance per pulse for the encoders
+    m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse);
+  }
+
+  /**
+   * 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);
+  }
+
+  /**
+   * 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.0;
+  }
+
+  /**
+   * 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);
+  }
+}

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

@@ -483,6 +483,19 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "ArmBot",
+    "description": "An example command-based robot demonstrating the use of a ProfiledPIDCommand to control an arm.",
+    "tags": [
+      "ArmBot",
+      "PID",
+      "Motion Profile"
+    ],
+    "foldername": "armbot",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "MecanumControllerCommand",
     "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",