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

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

@@ -0,0 +1,44 @@
+// 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 "Robot.h"
+
+#include "Constants.h"
+
+void Robot::RobotPeriodic() {
+  // Update the telemetry, including mechanism visualization, regardless of
+  // mode.
+  m_elevator.UpdateTelemetry();
+}
+
+void Robot::SimulationPeriodic() {
+  // Update the simulation model.
+  m_elevator.SimulationPeriodic();
+}
+
+void Robot::TeleopInit() {
+  // This just makes sure that our simulation code knows that the motor's off.
+  m_elevator.Reset();
+}
+
+void Robot::TeleopPeriodic() {
+  if (m_joystick.GetTrigger()) {
+    // Here, we set the constant setpoint of 0.75 meters.
+    m_elevator.ReachGoal(Constants::kSetpoint);
+  } else {
+    // Otherwise, we update the setpoint to 0.
+    m_elevator.ReachGoal(Constants::kLowerSetpoint);
+  }
+}
+
+void Robot::DisabledInit() {
+  // This just makes sure that our simulation code knows that the motor's off.
+  m_elevator.Stop();
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp

@@ -0,0 +1,64 @@
+// 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 "subsystems/Elevator.h"
+
+#include <frc/RobotController.h>
+#include <frc/StateSpaceUtil.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Elevator::Elevator() {
+  m_encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse);
+
+  // Put Mechanism 2d to SmartDashboard
+  // To view the Elevator visualization, select Network Tables -> SmartDashboard
+  // -> Elevator Sim
+  frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
+}
+
+void Elevator::SimulationPeriodic() {
+  // In this method, we update our simulation of what our elevator is doing
+  // First, we set our "inputs" (voltages)
+  m_elevatorSim.SetInput(frc::Vectord<1>{
+      m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()});
+
+  // Next, we update it. The standard loop time is 20ms.
+  m_elevatorSim.Update(20_ms);
+
+  // Finally, we set our simulated encoder's readings and simulated battery
+  // voltage
+  m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
+  // SimBattery estimates loaded battery voltages
+  frc::sim::RoboRioSim::SetVInVoltage(
+      frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
+}
+
+void Elevator::UpdateTelemetry() {
+  // Update the Elevator length based on the simulated elevator height
+  m_elevatorMech2d->SetLength(m_encoder.GetDistance());
+}
+
+void Elevator::ReachGoal(units::meter_t goal) {
+  frc::ExponentialProfile<units::meters, units::volts>::State goalState{goal,
+                                                                        0_mps};
+
+  auto next = m_profile.Calculate(20_ms, m_setpoint, goalState);
+
+  auto pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
+                                          m_setpoint.position / 1_m);
+  auto feedforwardOutput =
+      m_feedforward.Calculate(m_setpoint.velocity, next.velocity, 20_ms);
+
+  m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
+
+  m_setpoint = next;
+}
+
+void Elevator::Reset() {
+  m_setpoint = {m_encoder.GetDistance() * 1_m, 0_mps};
+}
+
+void Elevator::Stop() {
+  m_motor.Set(0.0);
+}

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

@@ -0,0 +1,57 @@
+// 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 <numbers>
+
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+/**
+ * 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 Constants {
+
+static constexpr int kMotorPort = 0;
+static constexpr int kEncoderAChannel = 0;
+static constexpr int kEncoderBChannel = 1;
+static constexpr int kJoystickPort = 0;
+
+static constexpr double kElevatorKp = 0.75;
+static constexpr double kElevatorKi = 0.0;
+static constexpr double kElevatorKd = 0.0;
+
+static constexpr units::volt_t kElevatorMaxV = 10_V;
+static constexpr units::volt_t kElevatorkS = 0.0_V;
+static constexpr units::volt_t kElevatorkG = 0.62_V;
+static constexpr auto kElevatorkV = 3.9_V / 1_mps;
+static constexpr auto kElevatorkA = 0.06_V / 1_mps_sq;
+
+static constexpr double kElevatorGearing = 5.0;
+static constexpr units::meter_t kElevatorDrumRadius = 1_in;
+static constexpr units::kilogram_t kCarriageMass = 12_lb;
+
+static constexpr units::meter_t kSetpoint = 42.875_in;
+static constexpr units::meter_t kLowerSetpoint = 15_in;
+static constexpr units::meter_t kMinElevatorHeight = 0_cm;
+static constexpr units::meter_t kMaxElevatorHeight = 50_in;
+
+// distance per pulse = (distance per revolution) / (pulses per revolution)
+//  = (Pi * D) / ppr
+static constexpr double kArmEncoderDistPerPulse =
+    2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
+
+}  // namespace Constants

wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Robot.h

@@ -0,0 +1,27 @@
+// 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/Joystick.h>
+#include <frc/TimedRobot.h>
+
+#include "subsystems/Elevator.h"
+
+/**
+ * This is a sample program to demonstrate the use of elevator simulation.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override {}
+  void RobotPeriodic() override;
+  void SimulationPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void DisabledInit() override;
+
+ private:
+  frc::Joystick m_joystick{Constants::kJoystickPort};
+  Elevator m_elevator;
+};

wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/subsystems/Elevator.h

@@ -0,0 +1,74 @@
+// 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/Encoder.h>
+#include <frc/controller/ElevatorFeedforward.h>
+#include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/ElevatorSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <frc/trajectory/ExponentialProfile.h>
+#include <units/length.h>
+
+#include "Constants.h"
+
+class Elevator {
+ public:
+  Elevator();
+  void SimulationPeriodic();
+  void UpdateTelemetry();
+  void ReachGoal(units::meter_t goal);
+  void Reset();
+  void Stop();
+
+ private:
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::NEO(2);
+
+  // Standard classes for controlling our elevator
+  frc::ExponentialProfile<units::meters, units::volts>::Constraints
+      m_constraints{Constants::kElevatorMaxV, Constants::kElevatorkV,
+                    Constants::kElevatorkA};
+  frc::ExponentialProfile<units::meters, units::volts> m_profile{m_constraints};
+  frc::ExponentialProfile<units::meters, units::volts>::State m_setpoint;
+
+  frc::PIDController m_controller{
+      Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd};
+
+  frc::ElevatorFeedforward m_feedforward{
+      Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV,
+      Constants::kElevatorkA};
+  frc::Encoder m_encoder{Constants::kEncoderAChannel,
+                         Constants::kEncoderBChannel};
+  frc::PWMSparkMax m_motor{Constants::kMotorPort};
+  frc::sim::PWMSim m_motorSim{m_motor};
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
+                                      Constants::kElevatorGearing,
+                                      Constants::kCarriageMass,
+                                      Constants::kElevatorDrumRadius,
+                                      Constants::kMinElevatorHeight,
+                                      Constants::kMaxElevatorHeight,
+                                      true,
+                                      0_m,
+                                      {0.005}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+  // Create a Mechanism2d display of an elevator
+  frc::Mechanism2d m_mech2d{10_in / 1_m, 51_in / 1_m};
+  frc::MechanismRoot2d* m_elevatorRoot =
+      m_mech2d.GetRoot("Elevator Root", 5_in / 1_m, 0.5_in / 1_m);
+  frc::MechanismLigament2d* m_elevatorMech2d =
+      m_elevatorRoot->Append<frc::MechanismLigament2d>(
+          "Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -205,6 +205,20 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "Elevator with exponential profiled PID",
+    "description": "Reach elevator position setpoints with exponential profiles and smart motor controller PID.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "Exponential Profile",
+      "Smart Motor Controller",
+      "Joystick"
+    ],
+    "foldername": "ElevatorExponentialProfile",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
   {
     "name": "Elevator with trapezoid profiled PID",
     "description": "Reach elevator position setpoints with trapezoid profiles and smart motor controller PID.",
@@ -784,6 +798,21 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "Elevator Exponential Profile Simulation",
+    "description": "Simulate an elevator.",
+    "tags": [
+      "Basic Robot",
+      "Elevator",
+      "State-Space",
+      "Simulation",
+      "Mechanism2d",
+      "Profiled PID"
+    ],
+    "foldername": "ElevatorExponentialSimulation",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
   {
     "name": "DifferentialDrivePoseEstimator",
     "description": "Combine differential-drive odometry with vision data using DifferentialDrivePoseEstimator.",