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

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

@@ -0,0 +1,46 @@
+// 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/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.
+ */
+
+static constexpr int kMotorPort = 0;
+static constexpr int kEncoderAChannel = 0;
+static constexpr int kEncoderBChannel = 1;
+static constexpr int kJoystickPort = 0;
+
+static constexpr std::string_view kArmPositionKey = "ArmPosition";
+static constexpr std::string_view kArmPKey = "ArmP";
+
+static constexpr double kDefaultArmKp = 50.0;
+static constexpr units::degree_t kDefaultArmSetpoint = 75.0_deg;
+
+static constexpr units::radian_t kMinAngle = -75.0_deg;
+static constexpr units::radian_t kMaxAngle = 255.0_deg;
+
+static constexpr double kArmReduction = 200.0;
+static constexpr units::kilogram_t kArmMass = 8.0_kg;
+static constexpr units::meter_t kArmLength = 30.0_in;
+
+// distance per pulse = (angle per revolution) / (pulses per revolution)
+//  = (2 * PI rads) / (4096 pulses)
+static constexpr double kArmEncoderDistPerPulse =
+    2.0 * std::numbers::pi / 4096.0;

wpilibcExamples/src/main/cpp/examples/ArmSimulation/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/Arm.h"
+
+/**
+ * This is a sample program to demonstrate the use of arm simulation.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override {}
+  void SimulationPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void DisabledInit() override;
+
+ private:
+  frc::Joystick m_joystick{kJoystickPort};
+  Arm m_arm;
+};

wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h

@@ -0,0 +1,67 @@
+// 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/ArmFeedforward.h>
+#include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/BatterySim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/PWMSim.h>
+#include <frc/simulation/RoboRioSim.h>
+#include <frc/simulation/SingleJointedArmSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <units/length.h>
+
+#include "Constants.h"
+
+class Arm {
+ public:
+  Arm();
+  void SimulationPeriodic();
+  void LoadPreferences();
+  void ReachSetpoint();
+  void Stop();
+
+ private:
+  // The P gain for the PID controller that drives this arm.
+  double m_armKp = kDefaultArmKp;
+  units::degree_t m_armSetpoint = kDefaultArmSetpoint;
+
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
+  frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2);
+
+  // Standard classes for controlling our arm
+  frc2::PIDController m_controller{m_armKp, 0, 0};
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+  frc::PWMSparkMax m_motor{kMotorPort};
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  // This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg,
+  // 30in overall arm length, range of motion in [-75, 255] degrees, and noise
+  // with a standard deviation of 1 encoder tick.
+  frc::sim::SingleJointedArmSim m_armSim{
+      m_armGearbox,
+      kArmReduction,
+      frc::sim::SingleJointedArmSim::EstimateMOI(kArmLength, kArmMass),
+      kArmLength,
+      kMinAngle,
+      kMaxAngle,
+      true,
+      {kArmEncoderDistPerPulse}};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+
+  // Create a Mechanism2d display of an Arm
+  frc::Mechanism2d m_mech2d{60, 60};
+  frc::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
+  frc::MechanismLigament2d* m_armTower =
+      m_armBase->Append<frc::MechanismLigament2d>(
+          "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue});
+  frc::MechanismLigament2d* m_arm = m_armBase->Append<frc::MechanismLigament2d>(
+      "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow});
+};