Add TrapezoidProfile external PID examples (#2131)

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

@@ -5,20 +5,22 @@
 /* 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 * wpi::math::pi * 1.5);
+    // 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 {
@@ -38,17 +40,18 @@ class Robot : public frc::TimedRobot {
     // 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<units::meters>::Constraints m_constraints{1.75_mps,
                                                                   0.75_mps_sq};

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