Add TrapezoidProfileCommand (#1962)

wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -68,6 +68,8 @@ class TrapezoidProfile {
   TrapezoidProfile(Constraints constraints, State goal,
                    State initial = State{0_m, 0_mps});
 
+  TrapezoidProfile(const TrapezoidProfile&) = default;
+  TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
   TrapezoidProfile(TrapezoidProfile&&) = default;
   TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
 

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

@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/controller/ProfiledPIDController.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a ProfiledPIDController.  Runs forever
+ * by default - to add exit conditions and/or other behavior, subclass this
+ * class. The controller calculation and output are performed synchronously in
+ * the command's execute() method.
+ *
+ * @see ProfiledPIDController
+ */
+class ProfiledPIDCommand
+    : public CommandHelper<CommandBase, ProfiledPIDCommand> {
+  using State = frc::TrapezoidProfile::State;
+
+ public:
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     std::function<State()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource   the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     std::function<units::meter_t()> goalSource,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     State goal, std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a
+   * ProfiledPIDController with a constant goal.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal         the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  ProfiledPIDCommand(frc::ProfiledPIDController controller,
+                     std::function<units::meter_t()> measurementSource,
+                     units::meter_t goal,
+                     std::function<void(double, State)> useOutput,
+                     std::initializer_list<Subsystem*> requirements);
+
+  ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
+
+  ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  /**
+   * Gets the measurement of the process variable. Wraps the passed-in function
+   * for readability.
+   *
+   * @return The measurement of the process variable
+   */
+  virtual units::meter_t GetMeasurement();
+
+  /**
+   * Gets the measurement of the process variable. Wraps the passed-in function
+   * for readability.
+   *
+   * @return The measurement of the process variable
+   */
+  virtual void UseOutput(double output, State state);
+
+  /**
+   * Returns the ProfiledPIDController used by the command.
+   *
+   * @return The ProfiledPIDController
+   */
+  frc::ProfiledPIDController& GetController();
+
+ protected:
+  frc::ProfiledPIDController m_controller;
+  std::function<units::meter_t()> m_measurement;
+  std::function<State()> m_goal;
+  std::function<void(double, State)> m_useOutput;
+};
+}  // namespace frc2

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

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/controller/ProfiledPIDController.h"
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a ProfiledPIDController to control an output.  The
+ * controller is run synchronously from the subsystem's periodic() method.
+ *
+ * @see ProfiledPIDController
+ */
+class ProfiledPIDSubsystem : public SubsystemBase {
+  using State = frc::TrapezoidProfile::State;
+
+ public:
+  /**
+   * Creates a new ProfiledPIDSubsystem.
+   *
+   * @param controller the ProfiledPIDController to use
+   */
+  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
+
+  void Periodic() override;
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   */
+  virtual void UseOutput(double output, State state) = 0;
+
+  /**
+   * Returns the goal used by the ProfiledPIDController.
+   *
+   * @return the goal to be used by the controller
+   */
+  virtual State GetGoal() = 0;
+
+  /**
+   * Returns the measurement of the process variable used by the
+   * ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  virtual units::meter_t GetMeasurement() = 0;
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  virtual void Enable();
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  virtual void Disable();
+
+  /**
+   * Returns the ProfiledPIDController.
+   *
+   * @return The controller.
+   */
+  frc::ProfiledPIDController& GetController();
+
+ protected:
+  frc::ProfiledPIDController m_controller;
+  bool m_enabled;
+};
+}  // namespace frc2

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

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Timer.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+
+#include <functional>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that runs a TrapezoidProfile.  Useful for smoothly controlling
+ * mechanism motion.
+ *
+ * @see TrapezoidProfile
+ */
+class TrapezoidProfileCommand
+    : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
+ public:
+  /**
+   * Creates a new TrapezoidProfileCommand that will execute the given
+   * TrapezoidalProfile. Output will be piped to the provided consumer function.
+   *
+   * @param profile The motion profile to execute.
+   * @param output  The consumer for the profile output.
+   */
+  TrapezoidProfileCommand(
+      frc::TrapezoidProfile profile,
+      std::function<void(frc::TrapezoidProfile::State)> output,
+      std::initializer_list<Subsystem*> requirements);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+ private:
+  frc::TrapezoidProfile m_profile;
+  std::function<void(frc::TrapezoidProfile::State)> m_output;
+
+  frc::Timer m_timer;
+};
+
+}  // namespace frc2