Add ProfiledPIDController with feedforward snippets

wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp

@@ -0,0 +1,54 @@
+// 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.
+
+#include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
+/**
+ * ProfiledPIDController with feedforward snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/profiled-pidcontroller.html
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  Robot() { m_encoder.SetDistancePerPulse(1.0 / 256.0); }
+
+  // Controls a simple motor's position using a SimpleMotorFeedforward
+  // and a ProfiledPIDController
+  void GoToPosition(units::meter_t goalPosition) {
+    auto pidVal = m_controller.Calculate(
+        units::meter_t{m_encoder.GetDistance()}, goalPosition);
+    m_motor.SetVoltage(units::volt_t{pidVal} +
+                       m_feedforward.Calculate(
+                           m_lastSpeed, m_controller.GetSetpoint().velocity));
+    m_lastSpeed = m_controller.GetSetpoint().velocity;
+  }
+
+  void TeleopPeriodic() override {
+    // Example usage: move to position 10.0 meters
+    GoToPosition(10.0_m);
+  }
+
+ private:
+  frc::ProfiledPIDController<units::meters> m_controller{
+      1.0, 0.0, 0.0, {5_mps, 10_mps_sq}};
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{0.5_V, 1.5_V / 1_mps,
+                                                           0.3_V / 1_mps_sq};
+  frc::Encoder m_encoder{0, 1};
+  frc::PWMSparkMax m_motor{0};
+
+  units::meters_per_second_t m_lastSpeed = 0_mps;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/snippets/snippets.json

@@ -154,5 +154,15 @@
     ],
     "foldername": "AccelerometerFilter",
     "gradlebase": "cpp"
+  },
+  {
+    "name": "ProfiledPIDFeedforward",
+    "description": "Snippets of ProfiledPIDController with feedforward for frc-docs.",
+    "tags": [
+      "PID",
+      "Profiled PID"
+    ],
+    "foldername": "ProfiledPIDFeedforward",
+    "gradlebase": "cpp"
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.snippets.profiledpidfeedforward;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java

@@ -0,0 +1,51 @@
+// 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.
+
+package edu.wpi.first.wpilibj.snippets.profiledpidfeedforward;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/**
+ * ProfiledPIDController with feedforward snippets for frc-docs.
+ * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/profiled-pidcontroller.html
+ */
+public class Robot extends TimedRobot {
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(5.0, 10.0));
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(0.5, 1.5, 0.3);
+  private final Encoder m_encoder = new Encoder(0, 1);
+  private final PWMSparkMax m_motor = new PWMSparkMax(0);
+
+  double m_lastSpeed;
+
+  /** Called once at the beginning of the robot program. */
+  public Robot() {
+    m_encoder.setDistancePerPulse(1.0 / 256.0);
+  }
+
+  /**
+   * Controls a simple motor's position using a SimpleMotorFeedforward and a ProfiledPIDController.
+   *
+   * @param goalPosition the desired position
+   */
+  public void goToPosition(double goalPosition) {
+    double pidVal = m_controller.calculate(m_encoder.getDistance(), goalPosition);
+    m_motor.setVoltage(
+        pidVal
+            + m_feedforward.calculateWithVelocities(
+                m_lastSpeed, m_controller.getSetpoint().velocity));
+    m_lastSpeed = m_controller.getSetpoint().velocity;
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Example usage: move to position 10.0
+    goToPosition(10.0);
+  }
+}

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

@@ -169,5 +169,16 @@
     "foldername": "accelerometerfilter",
     "gradlebase": "java",
     "mainclass": "Main"
+  },
+  {
+    "name": "ProfiledPIDFeedforward",
+    "description": "Snippets of ProfiledPIDController with feedforward for frc-docs.",
+    "tags": [
+      "PID",
+      "Profiled PID"
+    ],
+    "foldername": "profiledpidfeedforward",
+    "gradlebase": "java",
+    "mainclass": "Robot"
   }
 ]