[examples] Update ArmSimulation example to use Mechanism2d (#3572)

- Correct several comments that referenced elevator
- Changed noise to be 1 encoder tick instead of half a degree
- Changed gear ratio and PID value to be better tuned
- Updated bounds to be similar to a single jointed arm

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

@@ -12,7 +12,13 @@
 #include <frc/simulation/EncoderSim.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 <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/moment_of_inertia.h>
 #include <wpi/numbers>
@@ -28,14 +34,14 @@ class Robot : public frc::TimedRobot {
   static constexpr int kJoystickPort = 0;
 
   // The P gain for the PID controller that drives this arm.
-  static constexpr double kArmKp = 5.0;
+  static constexpr double kArmKp = 50.0;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
   static constexpr double kArmEncoderDistPerPulse =
       2.0 * wpi::numbers::pi / 4096.0;
 
-  // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+  // 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
@@ -45,24 +51,36 @@ class Robot : public frc::TimedRobot {
   frc::Joystick m_joystick{kJoystickPort};
 
   // Simulation classes help us simulate what's going on, including gravity.
-  // This sim represents an arm with 2 775s, a 100:1 reduction, a mass of 5kg,
-  // 30in overall arm length, range of motion nin [-180, 0] degrees, and noise
-  // with a standard deviation of 0.5 degrees.
+  // 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,
-      100.0,
+      600.0,
       frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 5_kg),
       30_in,
-      -180_deg,
-      0_deg,
+      -75_deg,
+      255_deg,
       5_kg,
       true,
-      {(0.5_deg).to<double>()}};
+      {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});
+
  public:
   void RobotInit() override {
     m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
   }
 
   void SimulationPeriodic() override {
@@ -80,13 +98,17 @@ class Robot : public frc::TimedRobot {
     // SimBattery estimates loaded battery voltages
     frc::sim::RoboRioSim::SetVInVoltage(
         frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm->SetAngle(m_armSim.GetAngle());
   }
 
   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>());
+      // Here, we run PID control like normal, with a constant setpoint of 75
+      // degrees.
+      double pidOutput = m_controller.Calculate(
+          m_encoder.GetDistance(), (units::radian_t(75_deg)).to<double>());
       m_motor.SetVoltage(units::volt_t(pidOutput));
     } else {
       // Otherwise, we disable the motor.

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

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

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

@@ -17,8 +17,14 @@ import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+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;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 
-/** This is a sample program to demonstrate the use of elevator simulation with existing code. */
+/** This is a sample program to demonstrate the use of arm simulation with existing code. */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
   private static final int kEncoderAChannel = 0;
@@ -26,49 +32,67 @@ public class Robot extends TimedRobot {
   private static final int kJoystickPort = 0;
 
   // The P gain for the PID controller that drives this arm.
-  private static final double kArmKp = 5.0;
+  private static final double kArmKp = 50.0;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
   private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
 
-  // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
   private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
 
-  // Standard classes for controlling our elevator
+  // Standard classes for controlling our arm
   private final PIDController m_controller = new PIDController(kArmKp, 0, 0);
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
   private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
   private final Joystick m_joystick = new Joystick(kJoystickPort);
 
   // Simulation classes help us simulate what's going on, including gravity.
-  private static final double m_armReduction = 100;
+  private static final double m_armReduction = 600;
   private static final double m_armMass = 5.0; // Kilograms
   private static final double m_armLength = Units.inchesToMeters(30);
-  // This arm sim represents an arm that can travel from -180 degrees (rotated straight backwards)
-  // to 0 degrees (rotated straight forwards).
+  // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+  // to 255 degrees (rotated down in the back).
   private final SingleJointedArmSim m_armSim =
       new SingleJointedArmSim(
           m_armGearbox,
           m_armReduction,
           SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
           m_armLength,
-          Units.degreesToRadians(-180),
-          Units.degreesToRadians(0),
+          Units.degreesToRadians(-75),
+          Units.degreesToRadians(255),
           m_armMass,
           true,
-          VecBuilder.fill(Units.degreesToRadians(0.5)) // Add noise with a std-dev of 0.5 degrees
+          VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
           );
   private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
 
+  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+  private final MechanismLigament2d m_armTower =
+      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+  private final MechanismLigament2d m_arm =
+      m_armPivot.append(
+          new MechanismLigament2d(
+              "Arm",
+              30,
+              Units.radiansToDegrees(m_armSim.getAngleRads()),
+              6,
+              new Color8Bit(Color.kYellow)));
+
   @Override
   public void robotInit() {
     m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    SmartDashboard.putData("Arm Sim", m_mech2d);
+    m_armTower.setColor(new Color8Bit(Color.kBlue));
   }
 
   @Override
   public void simulationPeriodic() {
-    // In this method, we update our simulation of what our elevator is doing
+    // In this method, we update our simulation of what our arm is doing
     // First, we set our "inputs" (voltages)
     m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
 
@@ -80,13 +104,16 @@ public class Robot extends TimedRobot {
     // SimBattery estimates loaded battery voltages
     RoboRioSim.setVInVoltage(
         BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
   }
 
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 30in.
-      var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(0));
+      // Here, we run PID control like normal, with a constant setpoint of 75 degrees.
+      var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(75));
       m_motor.setVoltage(pidOutput);
     } else {
       // Otherwise, we disable the motor.

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

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