[wpimath] Add Exponential motion profile (#5720)

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

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <numbers>
+
+#include <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/trajectory/ExponentialProfile.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+#include "ExampleSmartMotorController.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  static constexpr units::second_t kDt = 20_ms;
+
+  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)) {
+      m_goal = {5_m, 0_mps};
+    } else if (m_joystick.GetRawButtonPressed(3)) {
+      m_goal = {0_m, 0_mps};
+    }
+
+    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
+    // toward the goal while obeying the constraints.
+    auto next = m_profile.Calculate(kDt, m_goal, m_setpoint);
+
+    // Send setpoint to offboard controller PID
+    m_motor.SetSetpoint(
+        ExampleSmartMotorController::PIDMode::kPosition,
+        m_setpoint.position.value(),
+        m_feedforward.Calculate(m_setpoint.velocity, next.velocity, kDt) /
+            12_V);
+
+    m_setpoint = next;
+  }
+
+ private:
+  frc::Joystick m_joystick{1};
+  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_V / 1_mps, 1_V / 1_mps_sq};
+
+  // Create a motion profile with the given maximum velocity and maximum
+  // acceleration constraints for the next setpoint.
+  frc::ExponentialProfile<units::meters, units::volts> m_profile{
+      {10_V, 1_V / 1_mps, 1_V / 1_mps_sq}};
+  frc::ExponentialProfile<units::meters, units::volts>::State m_goal;
+  frc::ExponentialProfile<units::meters, units::volts>::State m_setpoint;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/motorcontrol/MotorController.h>
+
+/**
+ * A simplified stub class that simulates the API of a common "smart" motor
+ * controller.
+ *
+ * <p>Has no actual functionality.
+ */
+class ExampleSmartMotorController : public frc::MotorController {
+ 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 leader The leader to follow.
+   */
+  void Follow(ExampleSmartMotorController leader) {}
+
+  /**
+   * 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 {}
+};