diff --git a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
new file mode 100644
index 0000000..3cf84e7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <array>
+
+#include <frc/AddressableLED.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+  static constexpr int kLength = 60;
+
+  // PWM port 0
+  // Must be a PWM header, not MXP or DIO
+  frc::AddressableLED m_led{9};
+  std::array<frc::AddressableLED::LEDData, kLength>
+      m_ledBuffer;  // Reuse the buffer
+  // Store what the last hue of the first pixel is
+  int firstPixelHue = 0;
+
+ public:
+  void Rainbow() {
+    // For every pixel
+    for (int i = 0; i < kLength; i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      const auto pixelHue = (firstPixelHue + (i * 180 / kLength)) % 180;
+      // Set the value
+      m_ledBuffer[i].SetHSV(pixelHue, 255, 128);
+    }
+    // Increase by to make the rainbow "move"
+    firstPixelHue += 3;
+    // Check bounds
+    firstPixelHue %= 180;
+  }
+
+  void RobotInit() override {
+    // Default to a length of 60, start empty output
+    // Length is expensive to set, so only set it once, then just update data
+    m_led.SetLength(kLength);
+    m_led.SetData(m_ledBuffer);
+    m_led.Start();
+  }
+
+  void RobotPeriodic() override {
+    // Fill the buffer with a rainbow
+    Rainbow();
+    // Set the LEDs
+    m_led.SetData(m_ledBuffer);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
new file mode 100644
index 0000000..67f1e12
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with split arcade steering and an Xbox controller.
+ */
+class Robot : public frc::TimedRobot {
+  frc::PWMVictorSPX m_leftMotor{0};
+  frc::PWMVictorSPX m_rightMotor{1};
+  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::XboxController m_driverController{0};
+
+ public:
+  void TeleopPeriodic() {
+    // Drive with split arcade style
+    // That means that the Y axis of the left stick moves forward
+    // and backward, and the X of the right stick turns left and right.
+    m_robotDrive.ArcadeDrive(
+        m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+        m_driverController.GetX(frc::GenericHID::JoystickHand::kRightHand));
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/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
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aec6b1d
--- /dev/null
+++ b/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([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
new file mode 100644
index 0000000..5531c5b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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) {
+  m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+  // Start arm in neutral position
+  SetGoal(State{kArmOffset, 0_rad_per_s});
+}
+
+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);
+}
+
+units::radian_t ArmSubsystem::GetMeasurement() {
+  return units::radian_t(m_encoder.GetDistance()) + kArmOffset;
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..64be1b8
--- /dev/null
+++ b/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);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
new file mode 100644
index 0000000..674633c
--- /dev/null
+++ b/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
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
similarity index 60%
rename from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
rename to wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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.                                                               */
@@ -7,15 +7,17 @@
 
 #pragma once
 
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
 
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
 
-class Robot : public frc::IterativeRobot {
+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;
@@ -23,8 +25,9 @@
   void TestPeriodic() override;
 
  private:
-  frc::SendableChooser<std::string> m_chooser;
-  const std::string kAutoNameDefault = "Default";
-  const std::string kAutoNameCustom = "My Auto";
-  std::string m_autoSelected;
+  // 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;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
new file mode 100644
index 0000000..fa07359
--- /dev/null
+++ b/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();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
new file mode 100644
index 0000000..693f0b4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+  units::radian_t GetMeasurement() override;
+
+ private:
+  frc::PWMVictorSPX m_motor;
+  frc::Encoder m_encoder;
+  frc::ArmFeedforward m_feedforward;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/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;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..aec6b1d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
new file mode 100644
index 0000000..54a59ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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::TrapezoidProfileSubsystem<units::radians>(
+          {kMaxVelocity, kMaxAcceleration}, kArmOffset),
+      m_motor(kMotorPort),
+      m_feedforward(kS, kCos, kV, kA) {
+  m_motor.SetPID(kP, 0, 0);
+}
+
+void ArmSubsystem::UseState(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.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                      setpoint.position.to<double>(), feedforward / 12_V);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..47c898e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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.0;
+}
+
+frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
new file mode 100644
index 0000000..674633c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+  enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  explicit ExampleSmartMotorController(int port) {}
+
+  /**
+   * Example method for setting the PID gains of the smart controller.
+   *
+   * @param kp The proportional gain.
+   * @param ki The integral gain.
+   * @param kd The derivative gain.
+   */
+  void SetPID(double kp, double ki, double kd) {}
+
+  /**
+   * Example method for setting the setpoint of the smart controller in PID
+   * mode.
+   *
+   * @param mode The mode of the PID controller.
+   * @param setpoint The controller setpoint.
+   * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+   */
+  void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param master The master to follow.
+   */
+  void Follow(ExampleSmartMotorController master) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  double GetEncoderDistance() { return 0; }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  double GetEncoderRate() { return 0; }
+
+  /**
+   * Resets the encoder to zero distance.
+   */
+  void ResetEncoder() {}
+
+  void Set(double speed) override {}
+
+  double Get() const override { return 0; }
+
+  void SetInverted(bool isInverted) override {}
+
+  bool GetInverted() const override { return false; }
+
+  void Disable() override {}
+
+  void StopMotor() override {}
+
+  void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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.                                                               */
@@ -7,15 +7,17 @@
 
 #pragma once
 
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
 
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
 
-class Robot : public frc::IterativeRobot {
+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;
@@ -23,8 +25,9 @@
   void TestPeriodic() override;
 
  private:
-  frc::SendableChooser<std::string> m_chooser;
-  const std::string kAutoNameDefault = "Default";
-  const std::string kAutoNameCustom = "My Auto";
-  std::string m_autoSelected;
+  // 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;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
new file mode 100644
index 0000000..fa07359
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
new file mode 100644
index 0000000..52f15f1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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/controller/ArmFeedforward.h>
+#include <frc2/command/TrapezoidProfileSubsystem.h>
+#include <units/units.h>
+
+#include "ExampleSmartMotorController.h"
+
+/**
+ * A robot arm subsystem that moves with a motion profile.
+ */
+class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
+  using State = frc::TrapezoidProfile<units::radians>::State;
+
+ public:
+  ArmSubsystem();
+
+  void UseState(State setpoint) override;
+
+ private:
+  ExampleSmartMotorController m_motor;
+  frc::ArmFeedforward m_feedforward;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..3ed1357
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/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;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
new file mode 100644
index 0000000..a4030a5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/AnalogInput.h>
+#include <frc/DMA.h>
+#include <frc/DMASample.h>
+#include <frc/DigitalOutput.h>
+#include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+  frc::DMA m_dma;  // DMA object
+
+  // DMA needs a trigger, can use an output as trigger.
+  // 8 Triggers exist per DMA object, can be triggered on any
+  // DigitalSource.
+  frc::DigitalOutput m_dmaTrigger{2};
+
+  // Analog input to read with DMA
+  frc::AnalogInput m_analogInput{0};
+
+  // Encoder to read with DMA
+  frc::Encoder m_encoder{0, 1};
+
+ public:
+  void RobotInit() override {
+    // Trigger on falling edge of dma trigger output
+    m_dma.SetExternalTrigger(&m_dmaTrigger, false, true);
+
+    // Add inputs we want to read via DMA
+    m_dma.AddAnalogInput(&m_analogInput);
+    m_dma.AddEncoder(&m_encoder);
+    m_dma.AddEncoderPeriod(&m_encoder);
+
+    // Make sure trigger is set to off.
+    m_dmaTrigger.Set(true);
+
+    // Start DMA. No triggers or inputs can be added after this call
+    // unless DMA is stopped.
+    m_dma.StartDMA(1024);
+  }
+
+  void RobotPeriodic() override {
+    // Manually Trigger DMA read
+    m_dmaTrigger.Set(false);
+
+    // Need to create a sample.
+    frc::DMASample sample;
+    int32_t remaining = 0;
+    int32_t status = 0;
+    // Update our sample. remaining is the number of samples remaining in the
+    // buffer status is more specfic error messages if readStatus is not OK.
+    // Wait 1ms if buffer is empty
+    HAL_DMAReadStatus readStatus =
+        sample.Update(&m_dma, 1_ms, &remaining, &status);
+
+    if (readStatus == HAL_DMA_OK) {
+      // Status value in all these reads should be checked, a non 0 value means
+      // value could not be read
+
+      // If DMA is good, values exist
+      auto encoderDistance = sample.GetEncoderDistance(&m_encoder, &status);
+      // Period is not scaled, and is a raw value
+      auto encoderPeriod = sample.GetEncoderPeriodRaw(&m_encoder, &status);
+      auto analogVoltage =
+          sample.GetAnalogInputVoltage(&m_analogInput, &status);
+
+      frc::SmartDashboard::PutNumber("Distance", encoderDistance);
+      frc::SmartDashboard::PutNumber("Period", encoderPeriod);
+      frc::SmartDashboard::PutNumber("Input", analogVoltage);
+    }
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
index 8dcc1ea..4d9c1ed 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -7,11 +7,6 @@
 
 #include "Drivetrain.h"
 
-frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const {
-  return {units::meters_per_second_t(m_leftEncoder.GetRate()),
-          units::meters_per_second_t(m_rightEncoder.GetRate())};
-}
-
 void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
   const auto leftOutput = m_leftPIDController.Calculate(
       m_leftEncoder.GetRate(), speeds.left.to<double>());
@@ -28,5 +23,6 @@
 }
 
 void Drivetrain::UpdateOdometry() {
-  m_odometry.Update(GetAngle(), GetSpeeds());
+  m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
 }
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index bf2ad98..3032bd9 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -7,15 +7,14 @@
 
 #pragma once
 
-#include <units/units.h>
-
 #include <frc/AnalogGyro.h>
 #include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
 #include <frc/SpeedControllerGroup.h>
 #include <frc/controller/PIDController.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <units/units.h>
 #include <wpi/math>
 
 /**
@@ -32,6 +31,9 @@
                                       kEncoderResolution);
     m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
                                        kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
   }
 
   /**
@@ -47,7 +49,6 @@
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
       wpi::math::pi};  // 1/2 rotation per second
 
-  frc::DifferentialDriveWheelSpeeds GetSpeeds() const;
   void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
   void Drive(units::meters_per_second_t xSpeed,
              units::radians_per_second_t rot);
@@ -58,16 +59,16 @@
   static constexpr double kWheelRadius = 0.0508;  // meters
   static constexpr int kEncoderResolution = 4096;
 
-  frc::Spark m_leftMaster{1};
-  frc::Spark m_leftFollower{2};
-  frc::Spark m_rightMaster{3};
-  frc::Spark m_rightFollower{4};
+  frc::PWMVictorSPX m_leftMaster{1};
+  frc::PWMVictorSPX m_leftFollower{2};
+  frc::PWMVictorSPX m_rightMaster{3};
+  frc::PWMVictorSPX m_rightFollower{4};
 
   frc::SpeedControllerGroup m_leftGroup{m_leftMaster, m_leftFollower};
   frc::SpeedControllerGroup m_rightGroup{m_rightMaster, m_rightFollower};
 
   frc::Encoder m_leftEncoder{0, 1};
-  frc::Encoder m_rightEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
 
   frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
   frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
@@ -75,5 +76,5 @@
   frc::AnalogGyro m_gyro{0};
 
   frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{m_kinematics};
+  frc::DifferentialDriveOdometry m_odometry{GetAngle()};
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/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
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..78bb48a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "commands/DriveDistanceProfiled.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);
+
+  // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
+  // 10 seconds
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
+
+  // Do the same thing as above when the 'B' button is pressed, but defined
+  // inline
+  frc2::JoystickButton(&m_driverController, 2)
+      .WhenPressed(
+          frc2::TrapezoidProfileCommand<units::meters>(
+              frc::TrapezoidProfile<units::meters>(
+                  // Limit the max acceleration and velocity
+                  {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
+                  // End at desired position in meters; implicitly starts at 0
+                  {3_m, 0_mps}),
+              // Pipe the profile state to the drive
+              [this](auto setpointState) {
+                m_drive.SetDriveStates(setpointState, setpointState);
+              },
+              // Require the drive
+              {&m_drive})
+              .BeforeStarting([this]() { m_drive.ResetEncoders(); })
+              .WithTimeout(10_s));
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Runs the chosen command in autonomous
+  return new frc2::InstantCommand([] {});
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
new file mode 100644
index 0000000..7de607c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "commands/DriveDistanceProfiled.h"
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
+                                             DriveSubsystem* drive)
+    : CommandHelper(
+          frc::TrapezoidProfile<units::meters>(
+              // Limit the max acceleration and velocity
+              {kMaxSpeed, kMaxAcceleration},
+              // End at desired position in meters; implicitly starts at 0
+              {distance, 0_mps}),
+          // Pipe the profile state to the drive
+          [drive](auto setpointState) {
+            drive->SetDriveStates(setpointState, setpointState);
+          },
+          // Require the drive
+          {drive}) {
+  // Reset drive encoders since we're starting at 0
+  drive->ResetEncoders();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..cd8b3e4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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_leftMaster{kLeftMotor1Port},
+      m_leftSlave{kLeftMotor2Port},
+      m_rightMaster{kRightMotor1Port},
+      m_rightSlave{kRightMotor2Port},
+      m_feedforward{ks, kv, ka} {
+  m_leftSlave.Follow(m_leftMaster);
+  m_rightSlave.Follow(m_rightMaster);
+
+  m_leftMaster.SetPID(kp, 0, 0);
+  m_rightMaster.SetPID(kp, 0, 0);
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+}
+
+void DriveSubsystem::SetDriveStates(
+    frc::TrapezoidProfile<units::meters>::State left,
+    frc::TrapezoidProfile<units::meters>::State right) {
+  m_leftMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                           left.position.to<double>(),
+                           m_feedforward.Calculate(left.velocity) / 12_V);
+  m_rightMaster.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                            right.position.to<double>(),
+                            m_feedforward.Calculate(right.velocity) / 12_V);
+}
+
+void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
+  m_drive.ArcadeDrive(fwd, rot);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_leftMaster.ResetEncoder();
+  m_rightMaster.ResetEncoder();
+}
+
+units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
+  return units::meter_t(m_leftMaster.GetEncoderDistance());
+}
+
+units::meter_t DriveSubsystem::GetRightEncoderDistance() {
+  return units::meter_t(m_rightMaster.GetEncoderDistance());
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
new file mode 100644
index 0000000..bcf3356
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+// 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.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8_V * 1_s / 1_m;
+constexpr auto ka = 0.15_V * 1_s * 1_s / 1_m;
+
+constexpr double kp = 1;
+
+constexpr auto kMaxSpeed = 3_mps;
+constexpr auto kMaxAcceleration = 3_mps_sq;
+
+}  // namespace DriveConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+  enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  explicit ExampleSmartMotorController(int port) {}
+
+  /**
+   * Example method for setting the PID gains of the smart controller.
+   *
+   * @param kp The proportional gain.
+   * @param ki The integral gain.
+   * @param kd The derivative gain.
+   */
+  void SetPID(double kp, double ki, double kd) {}
+
+  /**
+   * Example method for setting the setpoint of the smart controller in PID
+   * mode.
+   *
+   * @param mode The mode of the PID controller.
+   * @param setpoint The controller setpoint.
+   * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+   */
+  void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param master The master to follow.
+   */
+  void Follow(ExampleSmartMotorController master) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  double GetEncoderDistance() { return 0; }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  double GetEncoderRate() { return 0; }
+
+  /**
+   * Resets the encoder to zero distance.
+   */
+  void ResetEncoder() {}
+
+  void Set(double speed) override {}
+
+  double Get() const override { return 0; }
+
+  void SetInverted(bool isInverted) override {}
+
+  bool GetInverted() const override { return false; }
+
+  void Disable() override {}
+
+  void StopMotor() override {}
+
+  void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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.                                                               */
@@ -7,15 +7,17 @@
 
 #pragma once
 
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
 
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
 
-class Robot : public frc::IterativeRobot {
+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;
@@ -23,8 +25,9 @@
   void TestPeriodic() override;
 
  private:
-  frc::SendableChooser<std::string> m_chooser;
-  const std::string kAutoNameDefault = "Default";
-  const std::string kAutoNameCustom = "My Auto";
-  std::string m_autoSelected;
+  // 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;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
new file mode 100644
index 0000000..af35be4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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/InstantCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/StartEndCommand.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(0.5); },
+                                        {}};
+  frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
+                                        {}};
+
+  void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
new file mode 100644
index 0000000..2199590
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc2/command/CommandHelper.h>
+#include <frc2/command/TrapezoidProfileCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+class DriveDistanceProfiled
+    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
+                                 DriveDistanceProfiled> {
+ public:
+  DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..f68cbbd
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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/controller/SimpleMotorFeedforward.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "ExampleSmartMotorController.h"
+
+class DriveSubsystem : public frc2::SubsystemBase {
+ public:
+  DriveSubsystem();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  // Subsystem methods go here.
+
+  /**
+   * Attempts to follow the given drive states using offboard PID.
+   *
+   * @param left The left wheel state.
+   * @param right The right wheel state.
+   */
+  void SetDriveStates(frc::TrapezoidProfile<units::meters>::State left,
+                      frc::TrapezoidProfile<units::meters>::State right);
+
+  /**
+   * 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 distance of the left encoder.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  units::meter_t GetLeftEncoderDistance();
+
+  /**
+   * Gets the distance of the right encoder.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  units::meter_t GetRightEncoderDistance();
+
+  /**
+   * 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
+  ExampleSmartMotorController m_leftMaster;
+  ExampleSmartMotorController m_leftSlave;
+  ExampleSmartMotorController m_rightMaster;
+  ExampleSmartMotorController m_rightSlave;
+
+  // A feedforward component for the drive
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward;
+
+  // The robot's drive
+  frc::DifferentialDrive m_drive{m_leftMaster, m_rightMaster};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
new file mode 100644
index 0000000..b75d99c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DutyCycleEncoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+  // Duty cycle encoder on channel 0
+  frc::DutyCycleEncoder m_dutyCycleEncoder{0};
+
+ public:
+  void RobotInit() override {
+    // Set to 0.5 units per rotation
+    m_dutyCycleEncoder.SetDistancePerRotation(0.5);
+  }
+
+  void RobotPeriodic() override {
+    // Connected can be checked, and uses the frequency of the encoder
+    auto connected = m_dutyCycleEncoder.IsConnected();
+
+    // Duty Cycle Frequency in Hz
+    auto frequency = m_dutyCycleEncoder.GetFrequency();
+
+    // Output of encoder
+    auto output = m_dutyCycleEncoder.Get();
+
+    // Output scaled by DistancePerPulse
+    auto distance = m_dutyCycleEncoder.GetDistance();
+
+    frc::SmartDashboard::PutBoolean("Connected", connected);
+    frc::SmartDashboard::PutNumber("Frequency", frequency);
+    frc::SmartDashboard::PutNumber("Output", output.to<double>());
+    frc::SmartDashboard::PutNumber("Distance", distance);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
new file mode 100644
index 0000000..1a113ae
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/DigitalInput.h>
+#include <frc/DutyCycle.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+class Robot : public frc::TimedRobot {
+  frc::DigitalInput m_input{0};         // Input channel
+  frc::DutyCycle m_dutyCycle{m_input};  // Duty cycle input
+
+ public:
+  void RobotInit() override {}
+
+  void RobotPeriodic() override {
+    // Duty Cycle Frequency in Hz
+    auto frequency = m_dutyCycle.GetFrequency();
+
+    // Output of duty cycle, ranging from 0 to 1
+    // 1 is fully on, 0 is fully off
+    auto output = m_dutyCycle.GetOutput();
+
+    frc::SmartDashboard::PutNumber("Frequency", frequency);
+    frc::SmartDashboard::PutNumber("Duty Cycle", output);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
index 95aac42..d423dd8 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -5,20 +5,22 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <units/units.h>
-
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
 #include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
 
 class Robot : public frc::TimedRobot {
  public:
   static constexpr units::second_t kDt = 20_ms;
 
-  Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+  Robot() {
+    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
+  }
 
   void TeleopPeriodic() override {
     if (m_joystick.GetRawButtonPressed(2)) {
@@ -39,8 +41,10 @@
 
   // Create a PID controller whose setpoint's change is subject to maximum
   // velocity and acceleration constraints.
-  frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
-  frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+                                                                  0.75_mps_sq};
+  frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
+                                                         m_constraints, kDt};
 };
 
 #ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index 9a15f31..5853054 100644
--- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -5,18 +5,23 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <frc/Encoder.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
-#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#include "ExampleSmartMotorController.h"
 
 class Robot : public frc::TimedRobot {
  public:
   static constexpr units::second_t kDt = 20_ms;
 
-  Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); }
+  Robot() {
+    // Note: These gains are fake, and will have to be tuned for your robot.
+    m_motor.SetPID(1.3, 0.0, 0.7);
+  }
 
   void TeleopPeriodic() override {
     if (m_joystick.GetRawButtonPressed(2)) {
@@ -28,27 +33,30 @@
     // Create a motion profile with the given maximum velocity and maximum
     // acceleration constraints for the next setpoint, the desired goal, and the
     // current setpoint.
-    frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+    frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
+                                                m_setpoint};
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.
     m_setpoint = profile.Calculate(kDt);
 
-    // Run controller with profiled setpoint and update motor output
-    double output = m_controller.Calculate(m_encoder.GetDistance(),
-                                           m_setpoint.position.to<double>());
-    m_motor.Set(output);
+    // Send setpoint to offboard controller PID
+    m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
+                        m_setpoint.position.to<double>(),
+                        m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
   }
 
  private:
   frc::Joystick m_joystick{1};
-  frc::Encoder m_encoder{1, 2};
-  frc::PWMVictorSPX m_motor{1};
-  frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
+  ExampleSmartMotorController m_motor{1};
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{
+      // Note: These gains are fake, and will have to be tuned for your robot.
+      1_V, 1.5_V * 1_s / 1_m};
 
-  frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State m_goal;
-  frc::TrapezoidProfile::State m_setpoint;
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
+                                                                  0.75_mps_sq};
+  frc::TrapezoidProfile<units::meters>::State m_goal;
+  frc::TrapezoidProfile<units::meters>::State m_setpoint;
 };
 
 #ifndef RUNNING_FRC_TESTS
diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
new file mode 100644
index 0000000..7c8d261
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/SpeedController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::SpeedController {
+ public:
+  enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
+
+  /**
+   * Creates a new ExampleSmartMotorController.
+   *
+   * @param port The port for the controller.
+   */
+  explicit ExampleSmartMotorController(int port) {}
+
+  /**
+   * Example method for setting the PID gains of the smart controller.
+   *
+   * @param kp The proportional gain.
+   * @param ki The integral gain.
+   * @param kd The derivative gain.
+   */
+  void SetPID(double kp, double ki, double kd) {}
+
+  /**
+   * Example method for setting the setpoint of the smart controller in PID
+   * mode.
+   *
+   * @param mode The mode of the PID controller.
+   * @param setpoint The controller setpoint.
+   * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
+   */
+  void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
+
+  /**
+   * Places this motor controller in follower mode.
+   *
+   * @param master The master to follow.
+   */
+  void Follow(ExampleSmartMotorController master) {}
+
+  /**
+   * Returns the encoder distance.
+   *
+   * @return The current encoder distance.
+   */
+  double GetEncoderDistance() { return 0; }
+
+  /**
+   * Returns the encoder rate.
+   *
+   * @return The current encoder rate.
+   */
+  double GetEncoderRate() { return 0; }
+
+  /**
+   * Resets the encoder to zero distance.
+   */
+  void ResetEncoder() {}
+
+  void Set(double speed) override {}
+
+  double Get() const override { return 0; }
+
+  void SetInverted(bool isInverted) override {}
+
+  bool GetInverted() const override { return false; }
+
+  void Disable() override {}
+
+  void StopMotor() override {}
+
+  void PIDWrite(double output) override {}
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
index 1dbbfa5..3cfffd7 100644
--- a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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.                                                               */
@@ -8,6 +8,7 @@
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+#include <wpi/math>
 
 /**
  * Sample program displaying the value of a quadrature encoder on the
@@ -42,7 +43,7 @@
      * inch diameter (1.5inch radius) wheel, and that we want to measure
      * distance in inches.
      */
-    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
 
     /* Defines the lowest rate at which the encoder will not be considered
      * stopped, for the purposes of the GetStopped() method. Units are in
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
 }
 
 double DriveSubsystem::GetAverageEncoderDistance() {
-  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
 frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
index d3633b6..85be752 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
@@ -17,29 +17,22 @@
     : PIDSubsystem(frc2::PIDController(kP, kI, kD)),
       m_shooterMotor(kShooterMotorPort),
       m_feederMotor(kFeederMotorPort),
-      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
-  m_controller.SetTolerance(kShooterToleranceRPS);
+      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
+      m_shooterFeedforward(kS, kV) {
+  m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
   m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  SetSetpoint(kShooterTargetRPS.to<double>());
 }
 
-void ShooterSubsystem::UseOutput(double output) {
-  // Use a feedforward of the form kS + kV * velocity
-  m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
-}
-
-void ShooterSubsystem::Disable() {
-  // Turn off motor when we disable, since useOutput(0) doesn't stop the motor
-  // due to our feedforward
-  frc2::PIDSubsystem::Disable();
-  m_shooterMotor.Set(0);
+void ShooterSubsystem::UseOutput(double output, double setpoint) {
+  m_shooterMotor.SetVoltage(units::volt_t(output) +
+                            m_shooterFeedforward.Calculate(kShooterTargetRPS));
 }
 
 bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
 
 double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
 
-double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
-
 void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
 
 void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
index 9fbcb27..1aef3ad 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -8,6 +8,7 @@
 #pragma once
 
 #include <units/units.h>
+#include <wpi/math>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -19,50 +20,50 @@
  */
 
 namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
 
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
 
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ShooterConstants {
-const int kEncoderPorts[]{4, 5};
-const bool kEncoderReversed = false;
-const int kEncoderCPR = 1024;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderPorts[]{4, 5};
+constexpr bool kEncoderReversed = false;
+constexpr int kEncoderCPR = 1024;
+constexpr double kEncoderDistancePerPulse =
     // Distance units will be rotations
-    1. / static_cast<double>(kEncoderCPR);
+    1.0 / static_cast<double>(kEncoderCPR);
 
-const int kShooterMotorPort = 4;
-const int kFeederMotorPort = 5;
+constexpr int kShooterMotorPort = 4;
+constexpr int kFeederMotorPort = 5;
 
-const double kShooterFreeRPS = 5300;
-const double kShooterTargetRPS = 4000;
-const double kShooterToleranceRPS = 50;
+constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
+constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
+constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
 
-const double kP = 1;
-const double kI = 0;
-const double kD = 0;
+constexpr double kP = 1;
+constexpr double kI = 0;
+constexpr double kD = 0;
 
 // On a real robot the feedforward constants should be empirically determined;
 // these are reasonable guesses.
-const double kSFractional = .05;
-const double kVFractional =
-    // Should have value 1 at free speed...
-    1. / kShooterFreeRPS;
+constexpr auto kS = 0.05_V;
+constexpr auto kV =
+    // Should have value 12V at free speed...
+    12_V / kShooterFreeRPS;
 
-const double kFeederSpeed = .5;
+constexpr double kFeederSpeed = 0.5;
 }  // namespace ShooterConstants
 
 namespace AutoConstants {
@@ -71,5 +72,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
index 2ce1e1c..76dff49 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
@@ -90,7 +90,7 @@
   frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); },
                                     {&m_shooter}};
 
-  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
                                         {}};
   frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
                                         {}};
diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
index 4d30d53..e8edd28 100644
--- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -9,20 +9,18 @@
 
 #include <frc/Encoder.h>
 #include <frc/PWMVictorSPX.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc2/command/PIDSubsystem.h>
+#include <units/units.h>
 
 class ShooterSubsystem : public frc2::PIDSubsystem {
  public:
   ShooterSubsystem();
 
-  void UseOutput(double output) override;
-
-  double GetSetpoint() override;
+  void UseOutput(double output, double setpoint) override;
 
   double GetMeasurement() override;
 
-  void Disable() override;
-
   bool AtSetpoint();
 
   void RunFeeder();
@@ -33,4 +31,5 @@
   frc::PWMVictorSPX m_shooterMotor;
   frc::PWMVictorSPX m_feederMotor;
   frc::Encoder m_shooterEncoder;
+  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
index a34c395..acdd07c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -28,8 +28,6 @@
 
 double Elevator::GetMeasurement() { return m_pot.Get(); }
 
-void Elevator::UseOutput(double d) { m_motor.Set(d); }
-
-double Elevator::GetSetpoint() { return m_setpoint; }
-
-void Elevator::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+void Elevator::UseOutput(double output, double setpoint) {
+  m_motor.Set(output);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
index 7f2dedc..eeb94bb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -26,10 +26,6 @@
   // frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
 }
 
-double Wrist::GetSetpoint() { return m_setpoint; }
-
-void Wrist::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
-
 double Wrist::GetMeasurement() { return m_pot.Get(); }
 
-void Wrist::UseOutput(double d) { m_motor.Set(d); }
+void Wrist::UseOutput(double output, double setpoint) { m_motor.Set(output); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
index a2c1e87..8acecbb 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -37,14 +37,7 @@
    * by
    * the subsystem.
    */
-  void UseOutput(double d) override;
-
-  double GetSetpoint() override;
-
-  /**
-   * Sets the setpoint for the subsystem.
-   */
-  void SetSetpoint(double setpoint);
+  void UseOutput(double output, double setpoint) override;
 
  private:
   frc::PWMVictorSPX m_motor{5};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
index e3ecc69..8b54e7e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -35,14 +35,7 @@
    * by
    * the subsystem.
    */
-  void UseOutput(double d) override;
-
-  double GetSetpoint() override;
-
-  /**
-   * Sets the setpoint for the subsystem.
-   */
-  void SetSetpoint(double setpoint);
+  void UseOutput(double output, double setpoint) override;
 
  private:
   frc::PWMVictorSPX m_motor{6};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
index aa1117a..6213333 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
@@ -8,6 +8,9 @@
 #include "RobotContainer.h"
 
 #include <frc/shuffleboard/Shuffleboard.h>
+#include <frc2/command/PIDCommand.h>
+#include <frc2/command/ParallelRaceGroup.h>
+#include <frc2/command/RunCommand.h>
 #include <frc2/command/button/JoystickButton.h>
 
 RobotContainer::RobotContainer() {
@@ -29,16 +32,39 @@
 void RobotContainer::ConfigureButtonBindings() {
   // Configure your button bindings here
 
+  // Assorted commands to be bound to buttons
+
   // Stabilize robot to drive straight with gyro when left bumper is held
-  frc2::JoystickButton(&m_driverController, 5).WhenHeld(&m_stabilizeDriving);
+  frc2::JoystickButton(&m_driverController, 5)
+      .WhenHeld(frc2::PIDCommand{
+          frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
+                              dc::kStabilizationD},
+          // Close the loop on the turn rate
+          [this] { return m_drive.GetTurnRate(); },
+          // Setpoint is 0
+          0,
+          // Pipe the output to the turning controls
+          [this](double output) {
+            m_drive.ArcadeDrive(m_driverController.GetY(
+                                    frc::GenericHID::JoystickHand::kLeftHand),
+                                output);
+          },
+          // Require the robot drive
+          {&m_drive}});
 
   // Turn to 90 degrees when the 'X' button is pressed
-  frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_turnTo90);
+  frc2::JoystickButton(&m_driverController, 3)
+      .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
+
+  // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
+  // second timeout
+  frc2::JoystickButton(&m_driverController, 1)
+      .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
 
   // While holding the shoulder button, drive at half speed
   frc2::JoystickButton(&m_driverController, 6)
-      .WhenPressed(&m_driveHalfSpeed)
-      .WhenReleased(&m_driveFullSpeed);
+      .WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
+      .WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
 }
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
index cadb7f9..988535c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
@@ -11,12 +11,12 @@
 
 using namespace DriveConstants;
 
-TurnToAngle::TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive)
+TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
     : CommandHelper(frc2::PIDController(kTurnP, kTurnI, kTurnD),
                     // Close loop on heading
-                    [drive] { return drive->GetHeading(); },
+                    [drive] { return drive->GetHeading().to<double>(); },
                     // Set reference to target
-                    targetAngleDegrees,
+                    target.to<double>(),
                     // Pipe output to turn robot
                     [drive](double output) { drive->ArcadeDrive(0, output); },
                     // Require the drive
@@ -26,9 +26,10 @@
   // Set the controller tolerance - the delta tolerance ensures the robot is
   // stationary at the setpoint before it is considered as having reached the
   // reference
-  m_controller.SetTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+  m_controller.SetTolerance(kTurnTolerance.to<double>(),
+                            kTurnRateTolerance.to<double>());
 
   AddRequirements({drive});
 }
 
-bool TurnToAngle::IsFinished() { return m_controller.AtSetpoint(); }
+bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
new file mode 100644
index 0000000..c2baa90
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "commands/TurnToAngleProfiled.h"
+
+#include <frc/controller/ProfiledPIDController.h>
+
+using namespace DriveConstants;
+
+TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
+                                         DriveSubsystem* drive)
+    : CommandHelper(
+          frc::ProfiledPIDController<units::radians>(
+              kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}),
+          // Close loop on heading
+          [drive] { return drive->GetHeading(); },
+          // Set reference to target
+          target,
+          // Pipe output to turn robot
+          [drive](double output, auto setpointState) {
+            drive->ArcadeDrive(0, output);
+          },
+          // Require the drive
+          {drive}) {
+  // Set the controller to be continuous (because it is an angle controller)
+  GetController().EnableContinuousInput(-180_deg, 180_deg);
+  // Set the controller tolerance - the delta tolerance ensures the robot is
+  // stationary at the setpoint before it is considered as having reached the
+  // reference
+  GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
+
+  AddRequirements({drive});
+}
+
+bool TurnToAngleProfiled::IsFinished() { return GetController().AtGoal(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
index 7e5ef96..c6d8d8c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
 }
 
 double DriveSubsystem::GetAverageEncoderDistance() {
-  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
 frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
@@ -46,10 +46,11 @@
   m_drive.SetMaxOutput(maxOutput);
 }
 
-double DriveSubsystem::GetHeading() {
-  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+units::degree_t DriveSubsystem::GetHeading() {
+  return units::degree_t(std::remainder(m_gyro.GetAngle(), 360) *
+                         (kGyroReversed ? -1.0 : 1.0));
 }
 
 double DriveSubsystem::GetTurnRate() {
-  return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+  return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
 }
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
index 0349c71..f423640 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -7,6 +7,9 @@
 
 #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.
@@ -17,42 +20,45 @@
  */
 
 namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
 
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
 
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 
-const bool kGyroReversed = true;
+constexpr bool kGyroReversed = true;
 
-const double kStabilizationP = 1;
-const double kStabilizationI = .5;
-const double kStabilizationD = 0;
+constexpr double kStabilizationP = 1;
+constexpr double kStabilizationI = 0.5;
+constexpr double kStabilizationD = 0;
 
-const double kTurnP = 1;
-const double kTurnI = 0;
-const double kTurnD = 0;
+constexpr double kTurnP = 1;
+constexpr double kTurnI = 0;
+constexpr double kTurnD = 0;
 
-const double kTurnToleranceDeg = 5;
-const double kTurnRateToleranceDegPerS = 10;  // degrees per second
+constexpr auto kTurnTolerance = 5_deg;
+constexpr auto kTurnRateTolerance = 10_deg_per_s;
+
+constexpr auto kMaxTurnRate = 100_deg_per_s;
+constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
 }  // namespace DriveConstants
 
 namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
index f480536..37c7e4f 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
@@ -12,9 +12,6 @@
 #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 "commands/TurnToAngle.h"
@@ -45,34 +42,6 @@
   // The robot's subsystems
   DriveSubsystem m_drive;
 
-  // Assorted commands to be bound to buttons
-
-  // Turn to 90 degrees, with a 5 second timeout
-  frc2::ParallelRaceGroup m_turnTo90 =
-      TurnToAngle{90, &m_drive}.WithTimeout(5_s);
-
-  // Stabilize the robot while driving
-  frc2::PIDCommand m_stabilizeDriving{
-      frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
-                          dc::kStabilizationD},
-      // Close the loop on the turn rate
-      [this] { return m_drive.GetTurnRate(); },
-      // Setpoint is 0
-      0,
-      // Pipe the output to the turning controls
-      [this](double output) {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
-            output);
-      },
-      // Require the robot drive
-      {&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;
 
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
index c7e875e..4afa20e 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
@@ -23,7 +23,7 @@
    * @param targetAngleDegrees The angle to turn to
    * @param drive              The drive subsystem to use
    */
-  TurnToAngle(double targetAngleDegrees, DriveSubsystem* drive);
+  TurnToAngle(units::degree_t target, DriveSubsystem* drive);
 
   bool IsFinished() override;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
new file mode 100644
index 0000000..0b52044
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc2/command/CommandHelper.h>
+#include <frc2/command/ProfiledPIDCommand.h>
+
+#include "subsystems/DriveSubsystem.h"
+
+/**
+ * A command that will turn the robot to the specified angle using a motion
+ * profile.
+ */
+class TurnToAngleProfiled
+    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
+                                 TurnToAngleProfiled> {
+ public:
+  /**
+   * Turns to robot to the specified angle using a motion profile.
+   *
+   * @param targetAngleDegrees The angle to turn to
+   * @param drive              The drive subsystem to use
+   */
+  TurnToAngleProfiled(units::degree_t targetAngleDegrees,
+                      DriveSubsystem* drive);
+
+  bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
index cada816..c4cd21c 100644
--- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -13,6 +13,7 @@
 #include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
 
 #include "Constants.h"
 
@@ -74,7 +75,7 @@
    *
    * @return the robot's heading in degrees, from 180 to 180
    */
-  double GetHeading();
+  units::degree_t GetHeading();
 
   /**
    * Returns the turn rate of the robot.
diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
index dedb0f2..ae5833e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
+++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* 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.                                                               */
@@ -18,7 +18,7 @@
 
 #include <stdio.h>
 
-#include "hal/HAL.h"
+#include <hal/HAL.h>
 
 enum DriverStationMode {
   DisabledMode,
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
 }
 
 double DriveSubsystem::GetAverageEncoderDistance() {
-  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
 frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
index b09572e..d465f6c 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#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.
@@ -17,34 +19,34 @@
  */
 
 namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
 
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
 
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace HatchConstants {
-const int kHatchSolenoidModule = 0;
-const int kHatchSolenoidPorts[]{0, 1};
+constexpr int kHatchSolenoidModule = 0;
+constexpr int kHatchSolenoidPorts[]{0, 1};
 }  // namespace HatchConstants
 
 namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
index b7c57a3..106812b 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
@@ -63,7 +63,7 @@
   frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}};
   frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); },
                                       {&m_hatch}};
-  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
                                         {}};
   frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
                                         {}};
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
index 839bb87..b35b2c6 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
@@ -10,6 +10,6 @@
 HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem)
     : m_drive(subsystem) {}
 
-void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(.5); }
+void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(0.5); }
 
 void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..47c898e 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
@@ -35,7 +35,7 @@
 }
 
 double DriveSubsystem::GetAverageEncoderDistance() {
-  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
 frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
index b09572e..d465f6c 100644
--- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#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.
@@ -17,34 +19,34 @@
  */
 
 namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
 
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
 
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace HatchConstants {
-const int kHatchSolenoidModule = 0;
-const int kHatchSolenoidPorts[]{0, 1};
+constexpr int kHatchSolenoidModule = 0;
+constexpr int kHatchSolenoidPorts[]{0, 1};
 }  // namespace HatchConstants
 
 namespace AutoConstants {
-const double kAutoDriveDistanceInches = 60;
-const double kAutoBackupDistanceInches = 20;
-const double kAutoDriveSpeed = .5;
+constexpr double kAutoDriveDistanceInches = 60;
+constexpr double kAutoBackupDistanceInches = 20;
+constexpr double kAutoDriveSpeed = 0.5;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
index 3d44730..6550085 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -9,7 +9,7 @@
 
 #include <frc/AnalogGyro.h>
 #include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
 #include <frc/controller/PIDController.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
@@ -45,15 +45,15 @@
       wpi::math::pi};  // 1/2 rotation per second
 
  private:
-  frc::Spark m_frontLeftMotor{1};
-  frc::Spark m_frontRightMotor{2};
-  frc::Spark m_backLeftMotor{3};
-  frc::Spark m_backRightMotor{4};
+  frc::PWMVictorSPX m_frontLeftMotor{1};
+  frc::PWMVictorSPX m_frontRightMotor{2};
+  frc::PWMVictorSPX m_backLeftMotor{3};
+  frc::PWMVictorSPX m_backRightMotor{4};
 
   frc::Encoder m_frontLeftEncoder{0, 1};
-  frc::Encoder m_frontRightEncoder{0, 1};
-  frc::Encoder m_backLeftEncoder{0, 1};
-  frc::Encoder m_backRightEncoder{0, 1};
+  frc::Encoder m_frontRightEncoder{2, 3};
+  frc::Encoder m_backLeftEncoder{4, 5};
+  frc::Encoder m_backRightEncoder{6, 7};
 
   frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
   frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
@@ -71,5 +71,5 @@
       m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
       m_backRightLocation};
 
-  frc::MecanumDriveOdometry m_odometry{m_kinematics};
+  frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()};
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/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
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..c7379cb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* 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/geometry/Translation2d.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/button/JoystickButton.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+const frc::MecanumDriveKinematics DriveConstants::kDriveKinematics{
+    frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
+    frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+    frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+    frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};
+
+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.Drive(m_driverController.GetY(frc::GenericHID::kLeftHand),
+                      m_driverController.GetX(frc::GenericHID::kRightHand),
+                      m_driverController.GetX(frc::GenericHID::kLeftHand),
+                      false);
+      },
+      {&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::MecanumControllerCommand mecanumControllerCommand(
+      exampleTrajectory, [this]() { return m_drive.GetPose(); },
+
+      frc::SimpleMotorFeedforward<units::meters>(ks, kv, ka),
+      DriveConstants::kDriveKinematics,
+
+      frc2::PIDController(AutoConstants::kPXController, 0, 0),
+      frc2::PIDController(AutoConstants::kPYController, 0, 0),
+      frc::ProfiledPIDController<units::radians>(
+          AutoConstants::kPThetaController, 0, 0,
+          AutoConstants::kThetaControllerConstraints),
+
+      AutoConstants::kMaxSpeed,
+
+      [this]() {
+        return frc::MecanumDriveWheelSpeeds{
+            units::meters_per_second_t(m_drive.GetFrontLeftEncoder().GetRate()),
+            units::meters_per_second_t(
+                m_drive.GetFrontRightEncoder().GetRate()),
+            units::meters_per_second_t(m_drive.GetRearLeftEncoder().GetRate()),
+            units::meters_per_second_t(
+                m_drive.GetRearRightEncoder().GetRate())};
+      },
+
+      frc2::PIDController(DriveConstants::kPFrontLeftVel, 0, 0),
+      frc2::PIDController(DriveConstants::kPRearLeftVel, 0, 0),
+      frc2::PIDController(DriveConstants::kPFrontRightVel, 0, 0),
+      frc2::PIDController(DriveConstants::kPRearRightVel, 0, 0),
+
+      [this](units::volt_t frontLeft, units::volt_t rearLeft,
+             units::volt_t frontRight, units::volt_t rearRight) {
+        m_drive.SetSpeedControllersVolts(frontLeft, rearLeft, frontRight,
+                                         rearRight);
+      },
+
+      {&m_drive});
+
+  // no auto
+  return new frc2::SequentialCommandGroup(
+      std::move(mecanumControllerCommand),
+      frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..b758b07
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/geometry/Rotation2d.h>
+#include <units/units.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+    : m_frontLeft{kFrontLeftMotorPort},
+      m_rearLeft{kRearLeftMotorPort},
+      m_frontRight{kFrontRightMotorPort},
+      m_rearRight{kRearRightMotorPort},
+
+      m_frontLeftEncoder{kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1],
+                         kFrontLeftEncoderReversed},
+      m_rearLeftEncoder{kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1],
+                        kRearLeftEncoderReversed},
+      m_frontRightEncoder{kFrontRightEncoderPorts[0],
+                          kFrontRightEncoderPorts[1],
+                          kFrontRightEncoderReversed},
+      m_rearRightEncoder{kRearRightEncoderPorts[0], kRearRightEncoderPorts[1],
+                         kRearRightEncoderReversed},
+
+      m_odometry{kDriveKinematics,
+                 frc::Rotation2d(units::degree_t(GetHeading())),
+                 frc::Pose2d()} {
+  // Set the distance per pulse for the encoders
+  m_frontLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+  m_odometry.Update(
+      frc::Rotation2d(units::degree_t(GetHeading())),
+      frc::MecanumDriveWheelSpeeds{
+          units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+          units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
+          units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+          units::meters_per_second_t(m_rearRightEncoder.GetRate())});
+}
+
+void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
+                           bool feildRelative) {
+  if (feildRelative) {
+    m_drive.DriveCartesian(ySpeed, xSpeed, rot, -m_gyro.GetAngle());
+  } else {
+    m_drive.DriveCartesian(ySpeed, xSpeed, rot);
+  }
+}
+
+void DriveSubsystem::SetSpeedControllersVolts(units::volt_t frontLeftPower,
+                                              units::volt_t rearLeftPower,
+                                              units::volt_t frontRightPower,
+                                              units::volt_t rearRightPower) {
+  m_frontLeft.SetVoltage(frontLeftPower);
+  m_rearLeft.SetVoltage(rearLeftPower);
+  m_frontRight.SetVoltage(frontRightPower);
+  m_rearRight.SetVoltage(rearRightPower);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_frontLeftEncoder.Reset();
+  m_rearLeftEncoder.Reset();
+  m_frontRightEncoder.Reset();
+  m_rearRightEncoder.Reset();
+}
+
+frc::Encoder& DriveSubsystem::GetFrontLeftEncoder() {
+  return m_frontLeftEncoder;
+}
+
+frc::Encoder& DriveSubsystem::GetRearLeftEncoder() { return m_rearLeftEncoder; }
+
+frc::Encoder& DriveSubsystem::GetFrontRightEncoder() {
+  return m_frontRightEncoder;
+}
+
+frc::Encoder& DriveSubsystem::GetRearRightEncoder() {
+  return m_rearRightEncoder;
+}
+
+frc::MecanumDriveWheelSpeeds DriveSubsystem::getCurrentWheelSpeeds() {
+  return (frc::MecanumDriveWheelSpeeds{
+      units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+      units::meters_per_second_t(m_rearLeftEncoder.GetRate()),
+      units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+      units::meters_per_second_t(m_rearRightEncoder.GetRate())});
+}
+
+void DriveSubsystem::SetMaxOutput(double maxOutput) {
+  m_drive.SetMaxOutput(maxOutput);
+}
+
+double DriveSubsystem::GetHeading() {
+  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+
+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,
+                           frc::Rotation2d(units::degree_t(GetHeading())));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
new file mode 100644
index 0000000..5f5f05f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#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 {
+constexpr int kFrontLeftMotorPort = 0;
+constexpr int kRearLeftMotorPort = 1;
+constexpr int kFrontRightMotorPort = 2;
+constexpr int kRearRightMotorPort = 3;
+
+constexpr int kFrontLeftEncoderPorts[]{0, 1};
+constexpr int kRearLeftEncoderPorts[]{2, 3};
+constexpr int kFrontRightEncoderPorts[]{4, 5};
+constexpr int kRearRightEncoderPorts[]{5, 6};
+
+constexpr bool kFrontLeftEncoderReversed = false;
+constexpr bool kRearLeftEncoderReversed = true;
+constexpr bool kFrontRightEncoderReversed = false;
+constexpr bool kRearRightEncoderReversed = true;
+
+constexpr auto kTrackWidth =
+    0.5_m;  // Distance between centers of right and left wheels on robot
+constexpr auto kWheelBase =
+    0.7_m;  // Distance between centers of front and back wheels on robot
+extern const frc::MecanumDriveKinematics kDriveKinematics;
+
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterMeters = .15;
+constexpr double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+
+constexpr bool kGyroReversed = false;
+
+// 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.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPFrontLeftVel = 0.5;
+constexpr double kPRearLeftVel = 0.5;
+constexpr double kPFrontRightVel = 0.5;
+constexpr double kPRearRightVel = 0.5;
+}  // namespace DriveConstants
+
+namespace AutoConstants {
+using radians_per_second_squared_t =
+    units::compound_unit<units::radians,
+                         units::inverse<units::squared<units::second>>>;
+
+constexpr auto kMaxSpeed = units::meters_per_second_t(3);
+constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
+constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3);
+constexpr auto kMaxAngularAcceleration =
+    units::unit_t<radians_per_second_squared_t>(3);
+
+constexpr double kPXController = 0.5;
+constexpr double kPYController = 0.5;
+constexpr double kPThetaController = 0.5;
+
+constexpr frc::TrapezoidProfile<units::radians>::Constraints
+    kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+}  // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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.                                                               */
@@ -7,15 +7,17 @@
 
 #pragma once
 
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
 
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
 
-class Robot : public frc::IterativeRobot {
+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;
@@ -23,8 +25,9 @@
   void TestPeriodic() override;
 
  private:
-  frc::SendableChooser<std::string> m_chooser;
-  const std::string kAutoNameDefault = "Default";
-  const std::string kAutoNameCustom = "My Auto";
-  std::string m_autoSelected;
+  // 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;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
new file mode 100644
index 0000000..3b4f6cc
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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/controller/ProfiledPIDController.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();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..fb3c19d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* 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/drive/MecanumDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/interfaces/Gyro.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.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 at given x, y and theta speeds. Speeds range from [-1, 1]
+   * and the linear speeds have no effect on the angular speed.
+   *
+   * @param xSpeed        Speed of the robot in the x direction
+   *                      (forward/backwards).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to
+   *                      the field.
+   */
+  void Drive(double xSpeed, double ySpeed, double rot, bool feildRelative);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the front left drive encoder.
+   *
+   * @return the front left drive encoder
+   */
+  frc::Encoder& GetFrontLeftEncoder();
+
+  /**
+   * Gets the rear left drive encoder.
+   *
+   * @return the rear left drive encoder
+   */
+  frc::Encoder& GetRearLeftEncoder();
+
+  /**
+   * Gets the front right drive encoder.
+   *
+   * @return the front right drive encoder
+   */
+  frc::Encoder& GetFrontRightEncoder();
+
+  /**
+   * Gets the rear right drive encoder.
+   *
+   * @return the rear right drive encoder
+   */
+  frc::Encoder& GetRearRightEncoder();
+
+  /**
+   * Gets the wheel speeds.
+   *
+   * @return the current wheel speeds.
+   */
+  frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
+
+  /**
+   * Sets the drive SpeedControllers to a desired voltage.
+   */
+  void SetSpeedControllersVolts(units::volt_t frontLeftPower,
+                                units::volt_t rearLeftPower,
+                                units::volt_t frontRightPower,
+                                units::volt_t rearRightPower);
+
+  /**
+   * 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();
+
+  /**
+   * Zeroes the heading of the robot.
+   */
+  void ZeroHeading();
+
+  /**
+   * 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_frontLeft;
+  frc::PWMVictorSPX m_rearLeft;
+  frc::PWMVictorSPX m_frontRight;
+  frc::PWMVictorSPX m_rearRight;
+
+  // The robot's drive
+  frc::MecanumDrive m_drive{m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
+
+  // The front-left-side drive encoder
+  frc::Encoder m_frontLeftEncoder;
+
+  // The rear-left-side drive encoder
+  frc::Encoder m_rearLeftEncoder;
+
+  // The front-right--side drive encoder
+  frc::Encoder m_frontRightEncoder;
+
+  // The rear-right-side drive encoder
+  frc::Encoder m_rearRightEncoder;
+
+  // The gyro sensor
+  frc::ADXRS450_Gyro m_gyro;
+
+  // Odometry class for tracking robot pose
+  frc::MecanumDriveOdometry m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index c5d22d0..71da410 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -12,7 +12,7 @@
 #include <frc/shuffleboard/Shuffleboard.h>
 #include <frc/trajectory/Trajectory.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
-#include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/RamseteCommand.h>
 #include <frc2/command/SequentialCommandGroup.h>
@@ -44,11 +44,19 @@
 }
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
+  // Create a voltage constraint to ensure we don't accelerate too fast
+  frc::DifferentialDriveVoltageConstraint autoVoltageConstraint(
+      frc::SimpleMotorFeedforward<units::meters>(
+          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka),
+      DriveConstants::kDriveKinematics, 10_V);
+
   // 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);
+  // Apply the voltage constraint
+  config.AddConstraint(autoVoltageConstraint);
 
   // An example trajectory to follow.  All units in meters.
   auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
@@ -65,23 +73,17 @@
       exampleTrajectory, [this]() { return m_drive.GetPose(); },
       frc::RamseteController(AutoConstants::kRamseteB,
                              AutoConstants::kRamseteZeta),
-      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka,
+      frc::SimpleMotorFeedforward<units::meters>(
+          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());
-      },
+      [this] { return m_drive.GetWheelSpeeds(); },
       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);
-      },
+      [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
       {&m_drive});
 
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(ramseteCommand),
-      frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
+      frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
 }
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index 3d5307f..be5f82a 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -7,8 +7,6 @@
 
 #include "subsystems/DriveSubsystem.h"
 
-#include <units/units.h>
-
 #include <frc/geometry/Rotation2d.h>
 #include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
 
@@ -21,26 +19,28 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
-      m_odometry{kDriveKinematics, frc::Pose2d()} {
+      m_odometry{frc::Rotation2d(units::degree_t(GetHeading()))} {
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+
+  ResetEncoders();
 }
 
 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())});
+                    units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
 }
 
 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::TankDriveVolts(units::volt_t left, units::volt_t right) {
+  m_leftMotors.SetVoltage(left);
+  m_rightMotors.SetVoltage(-right);
 }
 
 void DriveSubsystem::ResetEncoders() {
@@ -49,7 +49,7 @@
 }
 
 double DriveSubsystem::GetAverageEncoderDistance() {
-  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
 frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
@@ -61,15 +61,22 @@
 }
 
 double DriveSubsystem::GetHeading() {
-  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1.0 : 1.0);
 }
 
 double DriveSubsystem::GetTurnRate() {
-  return m_gyro.GetRate() * (kGyroReversed ? -1. : 1.);
+  return m_gyro.GetRate() * (kGyroReversed ? -1.0 : 1.0);
 }
 
 frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
 
+frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
+  return {units::meters_per_second_t(m_leftEncoder.GetRate()),
+          units::meters_per_second_t(m_rightEncoder.GetRate())};
+}
+
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  m_odometry.ResetPosition(pose);
+  ResetEncoders();
+  m_odometry.ResetPosition(pose,
+                           frc::Rotation2d(units::degree_t(GetHeading())));
 }
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index 801e479..22564c6 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -5,10 +5,10 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <units/units.h>
-
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
+#include <units/units.h>
+#include <wpi/math>
 
 #pragma once
 
@@ -22,50 +22,50 @@
  */
 
 namespace DriveConstants {
-const int kLeftMotor1Port = 0;
-const int kLeftMotor2Port = 1;
-const int kRightMotor1Port = 2;
-const int kRightMotor2Port = 3;
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
 
-const int kLeftEncoderPorts[]{0, 1};
-const int kRightEncoderPorts[]{2, 3};
-const bool kLeftEncoderReversed = false;
-const bool kRightEncoderReversed = true;
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
 
-const auto kTrackwidth = .6_m;
-const frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
+constexpr auto kTrackwidth = 0.6_m;
+constexpr frc::DifferentialDriveKinematics kDriveKinematics(kTrackwidth);
 
-const int kEncoderCPR = 1024;
-const double kWheelDiameterInches = 6;
-const double kEncoderDistancePerPulse =
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterInches = 6;
+constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * 3.142) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
 
-const bool kGyroReversed = true;
+constexpr 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;
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.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;
+constexpr double kPDriveVel = 0.5;
 }  // namespace DriveConstants
 
 namespace AutoConstants {
-const auto kMaxSpeed = 3_mps;
-const auto kMaxAcceleration = 3_mps_sq;
+constexpr auto kMaxSpeed = 3_mps;
+constexpr 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;
+constexpr double kRamseteB = 2;
+constexpr double kRamseteZeta = 0.7;
 }  // namespace AutoConstants
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
index cc91e0d..d6e4288 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
@@ -41,7 +41,7 @@
   // The robot's subsystems
   DriveSubsystem m_drive;
 
-  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
                                         {}};
   frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
                                         {}};
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
index 620cfd8..d8d1abd 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -15,6 +15,7 @@
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
 #include <frc2/command/SubsystemBase.h>
+#include <units/units.h>
 
 #include "Constants.h"
 
@@ -38,13 +39,12 @@
   void ArcadeDrive(double fwd, double rot);
 
   /**
-   * Drives the robot using tank controls.  Does not square inputs to enable
-   * composition with external controllers.
+   * Controls each side of the drive directly with a voltage.
    *
    * @param left the commanded left output
    * @param right the commanded right output
    */
-  void TankDrive(double left, double right);
+  void TankDriveVolts(units::volt_t left, units::volt_t right);
 
   /**
    * Resets the drive encoders to currently read a position of 0.
@@ -102,6 +102,13 @@
   frc::Pose2d GetPose();
 
   /**
+   * Returns the current wheel speeds of the robot.
+   *
+   * @return The current wheel speeds.
+   */
+  frc::DifferentialDriveWheelSpeeds GetWheelSpeeds();
+
+  /**
    * Resets the odometry to the specified pose.
    *
    * @param pose The pose to which to set the odometry.
diff --git a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
index 0a5f832..9fb2281 100644
--- a/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
@@ -18,5 +18,5 @@
  */
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
index 0a5f832..9fb2281 100644
--- a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
+++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
@@ -18,5 +18,5 @@
  */
 
 namespace OIConstants {
-const int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 1;
 }  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 697984c..f7a11b8 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -26,7 +26,8 @@
 
   // Limit the PID Controller's input range between -pi and pi and set the input
   // to be continuous.
-  m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
+  m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
+                                               units::radian_t(wpi::math::pi));
 }
 
 frc::SwerveModuleState SwerveModule::GetState() const {
@@ -41,10 +42,7 @@
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::meter_t(m_turningEncoder.Get()),
-      // We have to convert to the meters type here because that's what
-      // ProfiledPIDController wants.
-      units::meter_t(state.angle.Radians().to<double>()));
+      units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
 
   // Set the motor outputs.
   m_driveMotor.Set(driveOutput);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
index 745581a..2de6e95 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -57,5 +57,5 @@
       m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
       m_backRightLocation};
 
-  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics};
+  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()};
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
index 0eaa69e..0dafa24 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -8,7 +8,7 @@
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/Spark.h>
+#include <frc/PWMVictorSPX.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/kinematics/SwerveModuleState.h>
@@ -24,24 +24,19 @@
   static constexpr double kWheelRadius = 0.0508;
   static constexpr int kEncoderResolution = 4096;
 
-  // We have to use meters here instead of radians because of the fact that
-  // ProfiledPIDController's constraints only take in meters per second and
-  // meters per second squared.
+  static constexpr auto kModuleMaxAngularVelocity =
+      wpi::math::pi * 1_rad_per_s;  // radians per second
+  static constexpr auto kModuleMaxAngularAcceleration =
+      wpi::math::pi * 2_rad_per_s / 1_s;  // radians per second^2
 
-  static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
-      units::meters_per_second_t(wpi::math::pi);  // radians per second
-  static constexpr units::meters_per_second_squared_t
-      kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
-          wpi::math::pi * 2.0);  // radians per second squared
-
-  frc::Spark m_driveMotor;
-  frc::Spark m_turningMotor;
+  frc::PWMVictorSPX m_driveMotor;
+  frc::PWMVictorSPX m_turningMotor;
 
   frc::Encoder m_driveEncoder{0, 1};
-  frc::Encoder m_turningEncoder{0, 1};
+  frc::Encoder m_turningEncoder{2, 3};
 
   frc2::PIDController m_drivePIDController{1.0, 0, 0};
-  frc::ProfiledPIDController m_turningPIDController{
+  frc::ProfiledPIDController<units::radians> m_turningPIDController{
       1.0,
       0.0,
       0.0,
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
new file mode 100644
index 0000000..cd19aeb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/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
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..44efba5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* 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/geometry/Translation2d.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <frc2/command/InstantCommand.h>
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/SwerveControllerCommand.h>
+#include <frc2/command/button/JoystickButton.h>
+#include <units/units.h>
+
+#include "Constants.h"
+#include "subsystems/DriveSubsystem.h"
+
+using namespace DriveConstants;
+
+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.Drive(units::meters_per_second_t(
+                          m_driverController.GetY(frc::GenericHID::kLeftHand)),
+                      units::meters_per_second_t(
+                          m_driverController.GetY(frc::GenericHID::kRightHand)),
+                      units::radians_per_second_t(
+                          m_driverController.GetX(frc::GenericHID::kLeftHand)),
+                      false);
+      },
+      {&m_drive}));
+}
+
+void RobotContainer::ConfigureButtonBindings() {}
+
+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(m_drive.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::SwerveControllerCommand<4> swerveControllerCommand(
+      exampleTrajectory, [this]() { return m_drive.GetPose(); },
+
+      m_drive.kDriveKinematics,
+
+      frc2::PIDController(AutoConstants::kPXController, 0, 0),
+      frc2::PIDController(AutoConstants::kPYController, 0, 0),
+      frc::ProfiledPIDController<units::radians>(
+          AutoConstants::kPThetaController, 0, 0,
+          AutoConstants::kThetaControllerConstraints),
+
+      [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
+
+      {&m_drive});
+
+  // no auto
+  return new frc2::SequentialCommandGroup(
+      std::move(swerveControllerCommand), std::move(swerveControllerCommand),
+      frc2::InstantCommand(
+          [this]() {
+            m_drive.Drive(units::meters_per_second_t(0),
+                          units::meters_per_second_t(0),
+                          units::radians_per_second_t(0), false);
+          },
+          {}));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
new file mode 100644
index 0000000..f5d05ce
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/geometry/Rotation2d.h>
+#include <units/units.h>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+DriveSubsystem::DriveSubsystem()
+    : m_frontLeft{kFrontLeftDriveMotorPort,
+                  kFrontLeftTurningMotorPort,
+                  kFrontLeftDriveEncoderPorts,
+                  kFrontLeftTurningEncoderPorts,
+                  kFrontLeftDriveEncoderReversed,
+                  kFrontLeftTurningEncoderReversed},
+
+      m_rearLeft{
+          kRearLeftDriveMotorPort,       kRearLeftTurningMotorPort,
+          kRearLeftDriveEncoderPorts,    kRearLeftTurningEncoderPorts,
+          kRearLeftDriveEncoderReversed, kRearLeftTurningEncoderReversed},
+
+      m_frontRight{
+          kFrontRightDriveMotorPort,       kFrontRightTurningMotorPort,
+          kFrontRightDriveEncoderPorts,    kFrontRightTurningEncoderPorts,
+          kFrontRightDriveEncoderReversed, kFrontRightTurningEncoderReversed},
+
+      m_rearRight{
+          kRearRightDriveMotorPort,       kRearRightTurningMotorPort,
+          kRearRightDriveEncoderPorts,    kRearRightTurningEncoderPorts,
+          kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
+
+      m_odometry{kDriveKinematics,
+                 frc::Rotation2d(units::degree_t(GetHeading())),
+                 frc::Pose2d()} {}
+
+void DriveSubsystem::Periodic() {
+  // Implementation of subsystem periodic method goes here.
+  m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())),
+                    m_frontLeft.GetState(), m_rearLeft.GetState(),
+                    m_frontRight.GetState(), m_rearRight.GetState());
+}
+
+void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
+                           units::meters_per_second_t ySpeed,
+                           units::radians_per_second_t rot,
+                           bool fieldRelative) {
+  auto states = kDriveKinematics.ToSwerveModuleStates(
+      fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                          xSpeed, ySpeed, rot,
+                          frc::Rotation2d(units::degree_t(GetHeading())))
+                    : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+
+  kDriveKinematics.NormalizeWheelSpeeds(&states, AutoConstants::kMaxSpeed);
+
+  auto [fl, fr, bl, br] = states;
+
+  m_frontLeft.SetDesiredState(fl);
+  m_frontRight.SetDesiredState(fr);
+  m_rearLeft.SetDesiredState(bl);
+  m_rearRight.SetDesiredState(br);
+}
+
+void DriveSubsystem::SetModuleStates(
+    std::array<frc::SwerveModuleState, 4> desiredStates) {
+  kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
+                                        AutoConstants::kMaxSpeed);
+  m_frontLeft.SetDesiredState(desiredStates[0]);
+  m_rearLeft.SetDesiredState(desiredStates[1]);
+  m_frontRight.SetDesiredState(desiredStates[2]);
+  m_rearRight.SetDesiredState(desiredStates[3]);
+}
+
+void DriveSubsystem::ResetEncoders() {
+  m_frontLeft.ResetEncoders();
+  m_rearLeft.ResetEncoders();
+  m_frontRight.ResetEncoders();
+  m_rearRight.ResetEncoders();
+}
+
+double DriveSubsystem::GetHeading() {
+  return std::remainder(m_gyro.GetAngle(), 360) * (kGyroReversed ? -1. : 1.);
+}
+
+void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+
+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,
+                           frc::Rotation2d(units::degree_t(GetHeading())));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
new file mode 100644
index 0000000..22b3f0d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SwerveModule.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <wpi/math>
+
+#include "Constants.h"
+
+SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel,
+                           const int driveEncoderPorts[],
+                           const int turningEncoderPorts[],
+                           bool driveEncoderReversed,
+                           bool turningEncoderReversed)
+    : m_driveMotor(driveMotorChannel),
+      m_turningMotor(turningMotorChannel),
+      m_driveEncoder(driveEncoderPorts[0], driveEncoderPorts[1]),
+      m_turningEncoder(turningEncoderPorts[0], turningEncoderPorts[1]),
+      m_reverseDriveEncoder(driveEncoderReversed),
+      m_reverseTurningEncoder(turningEncoderReversed) {
+  // Set the distance per pulse for the drive encoder. We can simply use the
+  // distance traveled for one rotation of the wheel divided by the encoder
+  // resolution.
+  m_driveEncoder.SetDistancePerPulse(
+      ModuleConstants::kDriveEncoderDistancePerPulse);
+
+  // Set the distance (in this case, angle) per pulse for the turning encoder.
+  // This is the the angle through an entire rotation (2 * wpi::math::pi)
+  // divided by the encoder resolution.
+  m_turningEncoder.SetDistancePerPulse(
+      ModuleConstants::kTurningEncoderDistancePerPulse);
+
+  // Limit the PID Controller's input range between -pi and pi and set the input
+  // to be continuous.
+  m_turningPIDController.EnableContinuousInput(units::radian_t(-wpi::math::pi),
+                                               units::radian_t(wpi::math::pi));
+}
+
+frc::SwerveModuleState SwerveModule::GetState() {
+  return {units::meters_per_second_t{m_driveEncoder.GetRate()},
+          frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
+}
+
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& state) {
+  // Calculate the drive output from the drive PID controller.
+  const auto driveOutput = m_drivePIDController.Calculate(
+      m_driveEncoder.GetRate(), state.speed.to<double>());
+
+  // Calculate the turning motor output from the turning PID controller.
+  auto turnOutput = m_turningPIDController.Calculate(
+      units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
+
+  // Set the motor outputs.
+  m_driveMotor.Set(driveOutput);
+  m_turningMotor.Set(turnOutput);
+}
+
+void SwerveModule::ResetEncoders() {
+  m_driveEncoder.Reset();
+  m_turningEncoder.Reset();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
new file mode 100644
index 0000000..ed1f880
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -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 <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/units.h>
+#include <wpi/math>
+
+#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 {
+constexpr int kFrontLeftDriveMotorPort = 0;
+constexpr int kRearLeftDriveMotorPort = 2;
+constexpr int kFrontRightDriveMotorPort = 4;
+constexpr int kRearRightDriveMotorPort = 6;
+
+constexpr int kFrontLeftTurningMotorPort = 1;
+constexpr int kRearLeftTurningMotorPort = 3;
+constexpr int kFrontRightTurningMotorPort = 5;
+constexpr int kRearRightTurningMotorPort = 7;
+
+constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
+constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
+constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
+constexpr int kRearRightTurningEncoderPorts[2]{5, 6};
+
+constexpr bool kFrontLeftTurningEncoderReversed = false;
+constexpr bool kRearLeftTurningEncoderReversed = true;
+constexpr bool kFrontRightTurningEncoderReversed = false;
+constexpr bool kRearRightTurningEncoderReversed = true;
+
+constexpr int kFrontLeftDriveEncoderPorts[2]{0, 1};
+constexpr int kRearLeftDriveEncoderPorts[2]{2, 3};
+constexpr int kFrontRightDriveEncoderPorts[2]{4, 5};
+constexpr int kRearRightDriveEncoderPorts[2]{5, 6};
+
+constexpr bool kFrontLeftDriveEncoderReversed = false;
+constexpr bool kRearLeftDriveEncoderReversed = true;
+constexpr bool kFrontRightDriveEncoderReversed = false;
+constexpr bool kRearRightDriveEncoderReversed = true;
+
+constexpr bool kGyroReversed = false;
+
+// 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.
+constexpr auto ks = 1_V;
+constexpr auto kv = 0.8 * 1_V * 1_s / 1_m;
+constexpr auto ka = 0.15 * 1_V * 1_s * 1_s / 1_m;
+
+// Example value only - as above, this must be tuned for your drive!
+constexpr double kPFrontLeftVel = 0.5;
+constexpr double kPRearLeftVel = 0.5;
+constexpr double kPFrontRightVel = 0.5;
+constexpr double kPRearRightVel = 0.5;
+}  // namespace DriveConstants
+
+namespace ModuleConstants {
+constexpr int kEncoderCPR = 1024;
+constexpr double kWheelDiameterMeters = .15;
+constexpr double kDriveEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+
+constexpr double kTurningEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    (wpi::math::pi * 2) / static_cast<double>(kEncoderCPR);
+
+constexpr double kPModuleTurningController = 1;
+constexpr double kPModuleDriveController = 1;
+}  // namespace ModuleConstants
+
+namespace AutoConstants {
+using radians_per_second_squared_t =
+    units::compound_unit<units::radians,
+                         units::inverse<units::squared<units::second>>>;
+
+constexpr auto kMaxSpeed = units::meters_per_second_t(3);
+constexpr auto kMaxAcceleration = units::meters_per_second_squared_t(3);
+constexpr auto kMaxAngularSpeed = units::radians_per_second_t(3.142);
+constexpr auto kMaxAngularAcceleration =
+    units::unit_t<radians_per_second_squared_t>(3.142);
+
+constexpr double kPXController = 0.5;
+constexpr double kPYController = 0.5;
+constexpr double kPThetaController = 0.5;
+
+//
+
+constexpr frc::TrapezoidProfile<units::radians>::Constraints
+    kThetaControllerConstraints{kMaxAngularSpeed, kMaxAngularAcceleration};
+
+}  // namespace AutoConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 1;
+}  // namespace OIConstants
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
similarity index 60%
copy from wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
copy to wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
index 2c70b8f..fa173d3 100644
--- a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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.                                                               */
@@ -7,15 +7,17 @@
 
 #pragma once
 
-#include <string>
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
 
-#include <frc/IterativeRobot.h>
-#include <frc/smartdashboard/SendableChooser.h>
+#include "RobotContainer.h"
 
-class Robot : public frc::IterativeRobot {
+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;
@@ -23,8 +25,9 @@
   void TestPeriodic() override;
 
  private:
-  frc::SendableChooser<std::string> m_chooser;
-  const std::string kAutoNameDefault = "Default";
-  const std::string kAutoNameCustom = "My Auto";
-  std::string m_autoSelected;
+  // 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;
 };
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
new file mode 100644
index 0000000..8b36617
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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/controller/ProfiledPIDController.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;
+
+  // The chooser for the autonomous routines
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  void ConfigureButtonBindings();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
new file mode 100644
index 0000000..db4623f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* 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/drive/MecanumDrive.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/interfaces/Gyro.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+#include "SwerveModule.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 at given x, y and theta speeds. Speeds range from [-1, 1]
+   * and the linear speeds have no effect on the angular speed.
+   *
+   * @param xSpeed        Speed of the robot in the x direction
+   *                      (forward/backwards).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to
+   *                      the field.
+   */
+  void Drive(units::meters_per_second_t xSpeed,
+             units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+             bool feildRelative);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Sets the drive SpeedControllers to a power from -1 to 1.
+   */
+  void SetModuleStates(std::array<frc::SwerveModuleState, 4> desiredStates);
+
+  /**
+   * Returns the heading of the robot.
+   *
+   * @return the robot's heading in degrees, from 180 to 180
+   */
+  double GetHeading();
+
+  /**
+   * Zeroes the heading of the robot.
+   */
+  void ZeroHeading();
+
+  /**
+   * 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);
+
+  units::meter_t kTrackWidth =
+      .5_m;  // Distance between centers of right and left wheels on robot
+  units::meter_t kWheelBase =
+      .7_m;  // Distance between centers of front and back wheels on robot
+
+  frc::SwerveDriveKinematics<4> kDriveKinematics{
+      frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
+      frc::Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+      frc::Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+      frc::Translation2d(-kWheelBase / 2, -kTrackWidth / 2)};
+
+ private:
+  // Components (e.g. motor controllers and sensors) should generally be
+  // declared private and exposed only through public methods.
+
+  SwerveModule m_frontLeft;
+  SwerveModule m_rearLeft;
+  SwerveModule m_frontRight;
+  SwerveModule m_rearRight;
+
+  // The gyro sensor
+  frc::ADXRS450_Gyro m_gyro;
+
+  // Odometry class for tracking robot pose
+  // 4 defines the number of modules
+  frc::SwerveDriveOdometry<4> m_odometry;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
new file mode 100644
index 0000000..f8d816a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Spark.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <wpi/math>
+
+#include "Constants.h"
+
+class SwerveModule {
+  using radians_per_second_squared_t =
+      units::compound_unit<units::radians,
+                           units::inverse<units::squared<units::second>>>;
+
+ public:
+  SwerveModule(int driveMotorChannel, int turningMotorChannel,
+               const int driveEncoderPorts[2], const int turningEncoderPorts[2],
+               bool driveEncoderReversed, bool turningEncoderReversed);
+
+  frc::SwerveModuleState GetState();
+
+  void SetDesiredState(frc::SwerveModuleState& state);
+
+  void ResetEncoders();
+
+ private:
+  // We have to use meters here instead of radians due to the fact that
+  // ProfiledPIDController's constraints only take in meters per second and
+  // meters per second squared.
+
+  static constexpr units::radians_per_second_t kModuleMaxAngularVelocity =
+      units::radians_per_second_t(wpi::math::pi);  // radians per second
+  static constexpr units::unit_t<radians_per_second_squared_t>
+      kModuleMaxAngularAcceleration =
+          units::unit_t<radians_per_second_squared_t>(
+              wpi::math::pi * 2.0);  // radians per second squared
+
+  frc::Spark m_driveMotor;
+  frc::Spark m_turningMotor;
+
+  frc::Encoder m_driveEncoder;
+  frc::Encoder m_turningEncoder;
+
+  bool m_reverseDriveEncoder;
+  bool m_reverseTurningEncoder;
+
+  frc2::PIDController m_drivePIDController{
+      ModuleConstants::kPModuleDriveController, 0, 0};
+  frc::ProfiledPIDController<units::radians> m_turningPIDController{
+      ModuleConstants::kPModuleTurningController,
+      0.0,
+      0.0,
+      {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
new file mode 100644
index 0000000..3ca31ed
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/GenericHID.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with tank steering and an Xbox controller.
+ */
+class Robot : public frc::TimedRobot {
+  frc::PWMVictorSPX m_leftMotor{0};
+  frc::PWMVictorSPX m_rightMotor{1};
+  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::XboxController m_driverController{0};
+
+ public:
+  void TeleopPeriodic() {
+    // Drive with tank style
+    m_robotDrive.TankDrive(
+        m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
+        m_driverController.GetY(frc::GenericHID::JoystickHand::kRightHand));
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
index 2cc2131..9f9c01d 100644
--- a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
@@ -1,11 +1,12 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* 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 <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
 #include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
@@ -22,7 +23,10 @@
    */
   void TeleopPeriodic() override {
     // Sensor returns a value from 0-4095 that is scaled to inches
-    double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
+    // returned value is filtered with a rolling median filter, since
+    // ultrasonics tend to be quite noisy and susceptible to sudden outliers
+    double currentDistance =
+        m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches;
     // Convert distance error to a motor speed
     double currentSpeed = (kHoldDistance - currentDistance) * kP;
     // Drive robot
@@ -43,6 +47,9 @@
   static constexpr int kRightMotorPort = 1;
   static constexpr int kUltrasonicPort = 0;
 
+  // median filter to discard outliers; filters over 10 samples
+  frc::MedianFilter<double> m_filter{10};
+
   frc::AnalogInput m_ultrasonic{kUltrasonicPort};
 
   frc::PWMVictorSPX m_left{kLeftMotorPort};
diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
index 745fc10..47bd62d 100644
--- a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -6,6 +6,7 @@
 /*----------------------------------------------------------------------------*/
 
 #include <frc/AnalogInput.h>
+#include <frc/MedianFilter.h>
 #include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
@@ -27,7 +28,8 @@
   }
 
   void TeleopPeriodic() override {
-    double output = m_pidController.Calculate(m_ultrasonic.GetAverageVoltage());
+    double output =
+        m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
     m_robotDrive.ArcadeDrive(output, 0);
   }
 
@@ -51,6 +53,9 @@
   static constexpr int kRightMotorPort = 1;
   static constexpr int kUltrasonicPort = 0;
 
+  // median filter to discard outliers; filters over 5 samples
+  frc::MedianFilter<double> m_filter{5};
+
   frc::AnalogInput m_ultrasonic{kUltrasonicPort};
 
   frc::PWMVictorSPX m_left{kLeftMotorPort};
diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
index 864ea1d..4aa9f0e 100644
--- a/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -9,7 +9,8 @@
       "Complete List"
     ],
     "foldername": "MotorControl",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Motor Control With Encoder",
@@ -23,7 +24,8 @@
       "Complete List"
     ],
     "foldername": "MotorControlEncoder",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Relay",
@@ -34,7 +36,8 @@
       "Complete List"
     ],
     "foldername": "Relay",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "PDP CAN Monitoring",
@@ -45,7 +48,8 @@
       "Sensors"
     ],
     "foldername": "CANPDP",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Solenoids",
@@ -57,7 +61,8 @@
       "Complete List"
     ],
     "foldername": "Solenoid",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Encoder",
@@ -68,7 +73,8 @@
       "Sensors"
     ],
     "foldername": "Encoder",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Arcade Drive",
@@ -80,7 +86,8 @@
       "Complete List"
     ],
     "foldername": "ArcadeDrive",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Mecanum Drive",
@@ -92,7 +99,8 @@
       "Complete List"
     ],
     "foldername": "MecanumDrive",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Ultrasonic",
@@ -104,7 +112,8 @@
       "Analog"
     ],
     "foldername": "Ultrasonic",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "UltrasonicPID",
@@ -116,7 +125,8 @@
       "Analog"
     ],
     "foldername": "UltrasonicPID",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Gyro",
@@ -129,7 +139,8 @@
       "Joystick"
     ],
     "foldername": "Gyro",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Gyro Mecanum",
@@ -142,7 +153,8 @@
       "Joysitck"
     ],
     "foldername": "GyroMecanum",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "HID Rumble",
@@ -151,7 +163,8 @@
       "Joystick"
     ],
     "foldername": "HidRumble",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "PotentiometerPID",
@@ -164,7 +177,8 @@
       "Analog"
     ],
     "foldername": "PotentiometerPID",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Elevator with trapezoid profiled PID",
@@ -176,7 +190,8 @@
       "Joystick"
     ],
     "foldername": "ElevatorTrapezoidProfile",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Elevator with profiled PID controller",
@@ -188,7 +203,8 @@
       "Joystick"
     ],
     "foldername": "ElevatorProfiledPID",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Getting Started",
@@ -198,7 +214,8 @@
       "Complete List"
     ],
     "foldername": "GettingStarted",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Simple Vision",
@@ -208,7 +225,8 @@
       "Complete List"
     ],
     "foldername": "QuickVision",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Intermediate Vision",
@@ -218,7 +236,8 @@
       "Complete List"
     ],
     "foldername": "IntermediateVision",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Axis Camera Sample",
@@ -228,7 +247,8 @@
       "Complete List"
     ],
     "foldername": "AxisCameraSample",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "GearsBot",
@@ -238,7 +258,8 @@
       "Complete List"
     ],
     "foldername": "GearsBot",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "PacGoat",
@@ -248,7 +269,8 @@
       "Complete List"
     ],
     "foldername": "PacGoat",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "HAL",
@@ -257,7 +279,8 @@
       "HAL"
     ],
     "foldername": "HAL",
-    "gradlebase": "c"
+    "gradlebase": "c",
+    "commandversion": 1
   },
   {
     "name": "ShuffleBoard",
@@ -266,7 +289,8 @@
       "ShuffleBoard"
     ],
     "foldername": "ShuffleBoard",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "'Traditional' Hatchbot",
@@ -276,7 +300,8 @@
       "Command-based"
     ],
     "foldername": "HatchbotTraditional",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "'Inlined' Hatchbot",
@@ -287,7 +312,8 @@
       "Lambdas"
     ],
     "foldername": "HatchbotInlined",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "Select Command Example",
@@ -296,7 +322,8 @@
       "Command-based"
     ],
     "foldername": "SelectCommand",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "Scheduler Event Logging",
@@ -306,7 +333,8 @@
       "Shuffleboard"
     ],
     "foldername": "SchedulerEventLogging",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "Frisbeebot",
@@ -316,7 +344,8 @@
       "PID"
     ],
     "foldername": "Frisbeebot",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "Gyro Drive Commands",
@@ -327,7 +356,8 @@
       "Gyro"
     ],
     "foldername": "GyroDriveCommands",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "SwerveBot",
@@ -336,7 +366,8 @@
       "SwerveBot"
     ],
     "foldername": "SwerveBot",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "MecanumBot",
@@ -345,7 +376,8 @@
       "MecanumBot"
     ],
     "foldername": "MecanumBot",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
     "name": "DifferentialDriveBot",
@@ -354,10 +386,11 @@
       "DifferentialDriveBot"
     ],
     "foldername": "DifferentialDriveBot",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
   },
   {
-    "name:": "RamseteCommand",
+    "name": "RamseteCommand",
     "description": "An example command-based robot demonstrating the use of a RamseteCommand to follow a pregenerated trajectory.",
     "tags": [
       "RamseteCommand",
@@ -367,6 +400,137 @@
       "Path following"
     ],
     "foldername": "RamseteCommand",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "Arcade Drive Xbox Controller",
+    "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class and an Xbox Controller.",
+    "tags": [
+      "Getting Started with C++",
+      "Robot and Motor",
+      "XboxController",
+      "Complete List"
+    ],
+    "foldername": "ArcadeDriveXboxController",
+    "gradlebase": "cpp",
+    "commandversion": 1
+  },
+  {
+    "name": "Tank Drive Xbox Controller",
+    "description": "An example program which demonstrates the use of Tank Drive with the DifferentialDrive class and an Xbox Controller.",
+    "tags": [
+      "Getting Started with C++",
+      "Robot and Motor",
+      "XboxController",
+      "Complete List"
+    ],
+    "foldername": "TankDriveXboxController",
+    "gradlebase": "cpp",
+    "commandversion": 1
+  },
+  {
+    "name": "Duty Cycle Encoder",
+    "description": "Demonstrates the use of the Duty Cycle Encoder class",
+    "tags": [
+      "Getting Started with C++"
+    ],
+    "foldername": "DutyCycleEncoder",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "Duty Cycle Input",
+    "description": "Demonstrates the use of the Duty Cycle class",
+    "tags": [
+      "Getting Started with C++"
+    ],
+    "foldername": "DutyCycleInput",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "Addressable LED",
+    "description": "Demonstrates the use of the Addressable LED class",
+    "tags": [
+      "Getting Started with C++"
+    ],
+    "foldername": "AddressableLED",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "DMA",
+    "description": "Demonstrates the use of the DMA class",
+    "tags": [
+      "Advanced C++"
+    ],
+    "foldername": "DMA",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumControllerCommand",
+    "description": "An example command-based robot demonstrating the use of a MecanumControllerCommand to follow a pregenerated trajectory.",
+    "tags": [
+      "MecanumControllerCommand",
+      "Mecanum",
+      "PID",
+      "Trajectory",
+      "Path following"
+    ],
+    "foldername": "MecanumControllerCommand",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "SwerveControllerCommand",
+    "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.",
+    "tags": [
+      "SwerveControllerCommand",
+      "Swerve",
+      "PID",
+      "Trajectory",
+      "Path following"
+    ],
+    "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
+  },
+  {
+    "name": "ArmBotOffboard",
+    "description": "An example command-based robot demonstrating the use of a TrapezoidProfileSubsystem to control an arm with an offboard PID.",
+    "tags": [
+      "ArmBotOffboard",
+      "PID",
+      "Motion Profile"
+    ],
+    "foldername": "ArmBotOffboard",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "DriveDistanceOffboard",
+    "description": "An example command-based robot demonstrating the use of a TrapezoidProfileCommand to drive a robot a set distance with offboard PID on the drive.",
+    "tags": [
+      "DriveDistance",
+      "PID",
+      "Motion Profile"
+    ],
+    "foldername": "DriveDistanceOffboard",
+    "gradlebase": "cpp",
+    "commandversion": 2
   }
 ]
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
deleted file mode 100644
index 942fc9a..0000000
--- a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 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 <iostream>
-
-#include <frc/smartdashboard/SmartDashboard.h>
-
-void Robot::RobotInit() {
-  m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
-  m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
-  frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
-}
-
-/**
- * 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.
- */
-void Robot::RobotPeriodic() {}
-
-/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable chooser
- * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
- * remove all of the chooser code and uncomment the GetString line to get the
- * auto name from the text box below the Gyro.
- *
- * You can add additional auto modes by adding additional comparisons to the
- * if-else structure below with additional strings. If using the SendableChooser
- * make sure to add them to the chooser code above as well.
- */
-void Robot::AutonomousInit() {
-  m_autoSelected = m_chooser.GetSelected();
-  // m_autoSelected = SmartDashboard::GetString(
-  //     "Auto Selector", kAutoNameDefault);
-  std::cout << "Auto selected: " << m_autoSelected << std::endl;
-
-  if (m_autoSelected == kAutoNameCustom) {
-    // Custom Auto goes here
-  } else {
-    // Default Auto goes here
-  }
-}
-
-void Robot::AutonomousPeriodic() {
-  if (m_autoSelected == kAutoNameCustom) {
-    // Custom Auto goes here
-  } else {
-    // Default Auto goes here
-  }
-}
-
-void Robot::TeleopInit() {}
-
-void Robot::TeleopPeriodic() {}
-
-void Robot::TestPeriodic() {}
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
new file mode 100644
index 0000000..2a9ef50
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "OI.h"
+
+OI::OI() {
+  // Process operator interface input here.
+}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp
new file mode 100644
index 0000000..b2a8884
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.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 "Robot.h"
+
+#include <frc/commands/Scheduler.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+ExampleSubsystem Robot::m_subsystem;
+OI Robot::m_oi;
+
+void Robot::RobotInit() {
+  m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto);
+  m_chooser.AddOption("My Auto", &m_myAuto);
+  frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * 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.
+ */
+void Robot::RobotPeriodic() {}
+
+/**
+ * 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() { frc::Scheduler::GetInstance()->Run(); }
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString code to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons to
+ * the if-else structure below with additional strings & commands.
+ */
+void Robot::AutonomousInit() {
+  // std::string autoSelected = frc::SmartDashboard::GetString(
+  //     "Auto Selector", "Default");
+  // if (autoSelected == "My Auto") {
+  //   m_autonomousCommand = &m_myAuto;
+  // } else {
+  //   m_autonomousCommand = &m_defaultAuto;
+  // }
+
+  m_autonomousCommand = m_chooser.GetSelected();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Start();
+  }
+}
+
+void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+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;
+  }
+}
+
+void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
new file mode 100644
index 0000000..a5596ef
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "commands/ExampleCommand.h"
+
+#include "Robot.h"
+
+ExampleCommand::ExampleCommand() {
+  // Use Requires() here to declare subsystem dependencies
+  Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void ExampleCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ExampleCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool ExampleCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void ExampleCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ExampleCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
new file mode 100644
index 0000000..1389447
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "commands/MyAutoCommand.h"
+
+#include "Robot.h"
+
+MyAutoCommand::MyAutoCommand() {
+  // Use Requires() here to declare subsystem dependencies
+  Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void MyAutoCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void MyAutoCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool MyAutoCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void MyAutoCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void MyAutoCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
new file mode 100644
index 0000000..ed60550
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ExampleSubsystem.h"
+
+#include "RobotMap.h"
+
+ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {}
+
+void ExampleSubsystem::InitDefaultCommand() {
+  // Set the default command for a subsystem here.
+  // SetDefaultCommand(new MySpecialCommand());
+}
+
+// Put methods for controlling this subsystem
+// here. Call these from Commands.
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
new file mode 100644
index 0000000..77007bb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+
+class OI {
+ public:
+  OI();
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
new file mode 100644
index 0000000..94c2314
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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/TimedRobot.h>
+#include <frc/commands/Command.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+#include "OI.h"
+#include "commands/ExampleCommand.h"
+#include "commands/MyAutoCommand.h"
+#include "subsystems/ExampleSubsystem.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  static ExampleSubsystem m_subsystem;
+  static OI m_oi;
+
+  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.
+  frc::Command* m_autonomousCommand = nullptr;
+  ExampleCommand m_defaultAuto;
+  MyAutoCommand m_myAuto;
+  frc::SendableChooser<frc::Command*> m_chooser;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
new file mode 100644
index 0000000..875d3aa
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+
+// For example to map the left and right motors, you could define the
+// following variables to use with your drivetrain subsystem.
+// constexpr int kLeftMotor = 1;
+// constexpr int kRightMotor = 2;
+
+// If you are using multiple modules, make sure to define both the port
+// number and the module. For example you with a rangefinder:
+// constexpr int kRangeFinderPort = 1;
+// constexpr int kRangeFinderModule = 1;
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
new file mode 100644
index 0000000..558fced
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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/commands/Command.h>
+
+class ExampleCommand : public frc::Command {
+ public:
+  ExampleCommand();
+  void Initialize() override;
+  void Execute() override;
+  bool IsFinished() override;
+  void End() override;
+  void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
new file mode 100644
index 0000000..f1892a7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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/commands/Command.h>
+
+class MyAutoCommand : public frc::Command {
+ public:
+  MyAutoCommand();
+  void Initialize() override;
+  void Execute() override;
+  bool IsFinished() override;
+  void End() override;
+  void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
new file mode 100644
index 0000000..0f7d470
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* 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/commands/Subsystem.h>
+
+class ExampleSubsystem : public frc::Subsystem {
+ public:
+  ExampleSubsystem();
+  void InitDefaultCommand() override;
+
+ private:
+  // It's desirable that everything possible under private except
+  // for methods that implement subsystem capabilities
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
index 8495b63..de8fc3e 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
@@ -7,11 +7,10 @@
 
 #include "Robot.h"
 
-#include <hal/DriverStation.h>
-
 #include <frc/DriverStation.h>
 #include <frc/livewindow/LiveWindow.h>
 #include <frc/shuffleboard/Shuffleboard.h>
+#include <hal/DriverStation.h>
 #include <networktables/NetworkTable.h>
 
 void Robot::RobotInit() {}
@@ -32,7 +31,7 @@
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 
-  while (true) {
+  while (!m_exit) {
     if (IsDisabled()) {
       m_ds.InDisabled(true);
       Disabled();
@@ -61,6 +60,8 @@
   }
 }
 
+void Robot::EndCompetition() { m_exit = true; }
+
 #ifndef RUNNING_FRC_TESTS
 int main() { return frc::StartRobot<Robot>(); }
 #endif
diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
index 4efc087..0057778 100644
--- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
+++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <atomic>
+
 #include <frc/RobotBase.h>
 
 class Robot : public frc::RobotBase {
@@ -18,4 +20,8 @@
   void Test();
 
   void StartCompetition() override;
+  void EndCompetition() override;
+
+ private:
+  std::atomic<bool> m_exit{false};
 };
diff --git a/wpilibcExamples/src/main/cpp/templates/templates.json b/wpilibcExamples/src/main/cpp/templates/templates.json
index 71798c9..c31d41c 100644
--- a/wpilibcExamples/src/main/cpp/templates/templates.json
+++ b/wpilibcExamples/src/main/cpp/templates/templates.json
@@ -1,21 +1,13 @@
 [
   {
-    "name": "Iterative Robot",
-    "description": "Iterative style",
-    "tags": [
-      "Iterative"
-    ],
-    "foldername": "iterative",
-    "gradlebase": "cpp"
-  },
-  {
     "name": "Timed Robot",
     "description": "Timed style",
     "tags": [
       "Timed"
     ],
     "foldername": "timed",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Timed Skeleton (Advanced)",
@@ -24,7 +16,8 @@
       "Timed", "Skeleton"
     ],
     "foldername": "timedskeleton",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "RobotBase Skeleton (Advanced)",
@@ -33,7 +26,8 @@
       "RobotBase", "Skeleton"
     ],
     "foldername": "robotbaseskeleton",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 1
   },
   {
     "name": "Command Robot",
@@ -42,6 +36,17 @@
       "Command"
     ],
     "foldername": "commandbased",
-    "gradlebase": "cpp"
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
+    "name": "Old Command Robot",
+    "description": "Old-Command style (deprecated)",
+    "tags": [
+      "Command"
+    ],
+    "foldername": "oldcommandbased",
+    "gradlebase": "cpp",
+    "commandversion": 1
   }
 ]