[wpimath] Add Exponential motion profile (#5720)

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