[examples] Add Mechanism2d visualization to Elevator Sim (#3587)

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

@@ -5,6 +5,7 @@
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
 #include <frc/RobotController.h>
+#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
 #include <frc/motorcontrol/PWMSparkMax.h>
@@ -12,8 +13,15 @@
 #include <frc/simulation/ElevatorSim.h>
 #include <frc/simulation/EncoderSim.h>
 #include <frc/simulation/RoboRioSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/system/plant/LinearSystemId.h>
+#include <frc/util/Color.h>
+#include <frc/util/Color8Bit.h>
 #include <units/angle.h>
+#include <units/length.h>
 #include <units/moment_of_inertia.h>
 #include <wpi/numbers>
 
@@ -32,7 +40,7 @@ class Robot : public frc::TimedRobot {
   static constexpr units::meter_t kElevatorDrumRadius = 2_in;
   static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
 
-  static constexpr units::meter_t kMinElevatorHeight = 0_in;
+  static constexpr units::meter_t kMinElevatorHeight = 2_in;
   static constexpr units::meter_t kMaxElevatorHeight = 50_in;
 
   // distance per pulse = (distance per revolution) / (pulses per revolution)
@@ -59,9 +67,21 @@ class Robot : public frc::TimedRobot {
                                       {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", units::inch_t(m_elevatorSim.GetPosition()).to<double>(),
+          90_deg);
+
  public:
   void RobotInit() override {
     m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
   }
 
   void SimulationPeriodic() override {
@@ -79,13 +99,17 @@ class Robot : public frc::TimedRobot {
     // SimBattery estimates loaded battery voltages
     frc::sim::RoboRioSim::SetVInVoltage(
         frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
+
+    // Update the Elevator length based on the simulated elevator height
+    m_elevatorMech2d->SetLength(
+        units::inch_t(m_elevatorSim.GetPosition()).to<double>());
   }
 
   void TeleopPeriodic() override {
     if (m_joystick.GetTrigger()) {
       // Here, we run PID control like normal, with a constant setpoint of 30in.
-      double pidOutput =
-          m_controller.Calculate(m_encoder.GetDistance(), (30_in).to<double>());
+      double pidOutput = m_controller.Calculate(
+          m_encoder.GetDistance(), units::meter_t(30_in).to<double>());
       m_motor.SetVoltage(units::volt_t(pidOutput));
     } else {
       // Otherwise, we disable the motor.

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

@@ -653,7 +653,8 @@
       "Digital",
       "Sensors",
       "Simulation",
-      "Physics"
+      "Physics",
+      "Mechanism2d"
     ],
     "foldername": "ElevatorSimulation",
     "gradlebase": "cpp",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java

@@ -17,6 +17,10 @@ import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /** This is a sample program to demonstrate the use of elevator simulation with existing code. */
 public class Robot extends TimedRobot {
@@ -30,7 +34,7 @@ public class Robot extends TimedRobot {
   private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
   private static final double kCarriageMass = 4.0; // kg
 
-  private static final double kMinElevatorHeight = 0.0;
+  private static final double kMinElevatorHeight = Units.inchesToMeters(2);
   private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
 
   // distance per pulse = (distance per revolution) / (pulses per revolution)
@@ -58,9 +62,20 @@ public class Robot extends TimedRobot {
           VecBuilder.fill(0.01));
   private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
 
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
+  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(
+          new MechanismLigament2d(
+              "Elevator", Units.metersToInches(m_elevatorSim.getPositionMeters()), 90));
+
   @Override
   public void robotInit() {
     m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
   }
 
   @Override
@@ -77,6 +92,9 @@ public class Robot extends TimedRobot {
     // SimBattery estimates loaded battery voltages
     RoboRioSim.setVInVoltage(
         BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+
+    // Update elevator visualization with simulated position
+    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
   }
 
   @Override

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -694,7 +694,8 @@
       "Digital",
       "Sensors",
       "Simulation",
-      "Physics"
+      "Physics",
+      "Mechanism2d"
     ],
     "foldername": "elevatorsimulation",
     "gradlebase": "java",