Add TrapezoidProfile external PID examples (#2131)

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

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <units/units.h>
 #include <wpi/math>
 
 /**
@@ -45,8 +46,11 @@ constexpr double kTurnP = 1;
 constexpr double kTurnI = 0;
 constexpr double kTurnD = 0;
 
-constexpr double kTurnToleranceDeg = 5;
-constexpr 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 {

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 @@ class RobotContainer {
   // 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(0.5); },
-                                        {}};
-  frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
-                                        {}};
-
   // The chooser for the autonomous routines
   frc::SendableChooser<frc2::Command*> m_chooser;
 

wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h

@@ -23,7 +23,7 @@ class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
    * @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;
 };

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;
+};

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 @@ class DriveSubsystem : public frc2::SubsystemBase {
    *
    * @return the robot's heading in degrees, from 180 to 180
    */
-  double GetHeading();
+  units::degree_t GetHeading();
 
   /**
    * Returns the turn rate of the robot.