[examples] Update ArmSimulation example to use Preferences (#3976)

This shows more real world usage then hardcoding the setpoint and PID
gains. There were no current examples using Preferences. This will also
be used to update frc-docs article for Preferences.

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

@@ -4,6 +4,7 @@
 
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
+#include <frc/Preferences.h>
 #include <frc/RobotController.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
@@ -33,8 +34,13 @@ class Robot : public frc::TimedRobot {
   static constexpr int kEncoderBChannel = 1;
   static constexpr int kJoystickPort = 0;
 
+  static constexpr std::string_view kArmPositionKey = "ArmPosition";
+  static constexpr std::string_view kArmPKey = "ArmP";
+
   // The P gain for the PID controller that drives this arm.
-  static constexpr double kArmKp = 50.0;
+  double kArmKp = 50.0;
+
+  units::degree_t armPosition = 75.0_deg;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
@@ -81,6 +87,15 @@ class Robot : public frc::TimedRobot {
 
     // Put Mechanism 2d to SmartDashboard
     frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
+
+    // Set the Arm position setpoint and P constant to Preferences if the keys
+    // don't already exist
+    if (!frc::Preferences::ContainsKey(kArmPositionKey)) {
+      frc::Preferences::SetDouble(kArmPositionKey, armPosition.value());
+    }
+    if (!frc::Preferences::ContainsKey(kArmPKey)) {
+      frc::Preferences::SetDouble(kArmPKey, armPosition.value());
+    }
   }
 
   void SimulationPeriodic() override {
@@ -103,12 +118,22 @@ class Robot : public frc::TimedRobot {
     m_arm->SetAngle(m_armSim.GetAngle());
   }
 
+  void TeleopInit() override {
+    // Read Preferences for Arm setpoint and kP on entering Teleop
+    armPosition = units::degree_t(
+        frc::Preferences::GetDouble(kArmPositionKey, armPosition.value()));
+    if (kArmKp != frc::Preferences::GetDouble(kArmPKey, kArmKp)) {
+      kArmKp = frc::Preferences::GetDouble(kArmPKey, kArmKp);
+      m_controller.SetP(kArmKp);
+    }
+  }
+
   void TeleopPeriodic() override {
     if (m_joystick.GetTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 75
-      // degrees.
+      // Here, we run PID control like normal, with a setpoint read from
+      // preferences in degrees.
       double pidOutput = m_controller.Calculate(
-          m_encoder.GetDistance(), (units::radian_t(75_deg)).value());
+          m_encoder.GetDistance(), (units::radian_t(armPosition).value()));
       m_motor.SetVoltage(units::volt_t(pidOutput));
     } else {
       // Otherwise, we disable the motor.

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

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