[examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052)

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

@@ -0,0 +1,55 @@
+// 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 = 5.0;
+static constexpr double kElevatorKi = 0.0;
+static constexpr double kElevatorKd = 0.0;
+
+static constexpr units::volt_t kElevatorkS = 0.0_V;
+static constexpr units::volt_t kElevatorkG = 0.762_V;
+static constexpr auto kElevatorkV = 0.762_V / 1_mps;
+static constexpr auto kElevatorkA = 0.0_V / 1_mps_sq;
+
+static constexpr double kElevatorGearing = 10.0;
+static constexpr units::meter_t kElevatorDrumRadius = 2_in;
+static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+
+static constexpr units::meter_t kSetpoint = 75_cm;
+static constexpr units::meter_t kMinElevatorHeight = 0_cm;
+static constexpr units::meter_t kMaxElevatorHeight = 1.25_m;
+
+// 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/ElevatorSimulation/include/Robot.h

@@ -0,0 +1,26 @@
+// 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 TeleopPeriodic() override;
+  void DisabledInit() override;
+
+ private:
+  frc::Joystick m_joystick{Constants::kJoystickPort};
+  Elevator m_elevator;
+};

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

@@ -0,0 +1,69 @@
+// 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/controller/ProfiledPIDController.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 <units/length.h>
+
+#include "Constants.h"
+
+class Elevator {
+ public:
+  Elevator();
+  void SimulationPeriodic();
+  void UpdateTelemetry();
+  void ReachGoal(units::meter_t goal);
+  void Stop();
+
+ private:
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{2.45_mps,
+                                                                  2.45_mps_sq};
+  frc::ProfiledPIDController<units::meters> m_controller{
+      Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd,
+      m_constraints};
+
+  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.01}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+  // Create a Mechanism2d display of an elevator
+  frc::Mechanism2d m_mech2d{20, 50};
+  frc::MechanismRoot2d* m_elevatorRoot =
+      m_mech2d.GetRoot("Elevator Root", 10, 0);
+  frc::MechanismLigament2d* m_elevatorMech2d =
+      m_elevatorRoot->Append<frc::MechanismLigament2d>(
+          "Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
+};