From 17344d8891d0121dabac876730496828554a03bc Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 7 Oct 2025 21:34:52 -0500 Subject: [PATCH 1/9] Add ProfiledPIDController with feedforward snippets Adds snippets demonstrating ProfiledPIDController usage with SimpleMotorFeedforward using the two-parameter calculate() method (currentVelocity, nextVelocity). These snippets will be used in frc-docs to document the recommended feedforward pattern with ProfiledPIDController. --- .../ProfiledPIDFeedforward/cpp/Robot.cpp | 53 +++++++++++++++++++ .../profiledpidfeedforward/Robot.java | 47 ++++++++++++++++ 2 files changed, 100 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java diff --git a/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp new file mode 100644 index 00000000000..f132bbd2b3c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp @@ -0,0 +1,53 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * 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( + 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 m_controller{ + 1.0, 0.0, 0.0, {5_mps, 10_mps_sq}}; + frc::SimpleMotorFeedforward 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(); +} +#endif diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java new file mode 100644 index 00000000000..7c37771a3c9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java @@ -0,0 +1,47 @@ +// 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 = 0; + + /** 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 + public void goToPosition(double goalPosition) { + double pidVal = m_controller.calculate(m_encoder.getDistance(), goalPosition); + m_motor.setVoltage( + pidVal + + m_feedforward.calculate(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); + } +} From c746e6bf09b0e9b0a381c43b19b0f930a2eb3cb9 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 7 Oct 2025 21:52:41 -0500 Subject: [PATCH 2/9] Apply wpiformat and spotless formatting --- .../cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp | 11 ++++++----- .../snippets/profiledpidfeedforward/Robot.java | 3 +-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp index f132bbd2b3c..dda857a2c4d 100644 --- a/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp @@ -23,10 +23,11 @@ class Robot : public frc::TimedRobot { // 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( - pidVal + - m_feedforward.Calculate(m_lastSpeed, m_controller.GetSetpoint().velocity)); + auto pidVal = m_controller.Calculate( + units::meter_t{m_encoder.GetDistance()}, goalPosition); + m_motor.SetVoltage(pidVal + + m_feedforward.Calculate( + m_lastSpeed, m_controller.GetSetpoint().velocity)); m_lastSpeed = m_controller.GetSetpoint().velocity; } @@ -39,7 +40,7 @@ class Robot : public frc::TimedRobot { frc::ProfiledPIDController m_controller{ 1.0, 0.0, 0.0, {5_mps, 10_mps_sq}}; frc::SimpleMotorFeedforward m_feedforward{0.5_V, 1.5_V / 1_mps, - 0.3_V / 1_mps_sq}; + 0.3_V / 1_mps_sq}; frc::Encoder m_encoder{0, 1}; frc::PWMSparkMax m_motor{0}; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java index 7c37771a3c9..789d476ebc4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java @@ -34,8 +34,7 @@ public Robot() { public void goToPosition(double goalPosition) { double pidVal = m_controller.calculate(m_encoder.getDistance(), goalPosition); m_motor.setVoltage( - pidVal - + m_feedforward.calculate(m_lastSpeed, m_controller.getSetpoint().velocity)); + pidVal + m_feedforward.calculate(m_lastSpeed, m_controller.getSetpoint().velocity)); m_lastSpeed = m_controller.getSetpoint().velocity; } From 98906593422c0773f319474a99a2b22ce8236240 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 7 Oct 2025 22:13:51 -0500 Subject: [PATCH 3/9] Register ProfiledPIDFeedforward snippets in snippets.json files --- wpilibcExamples/src/main/cpp/snippets/snippets.json | 11 +++++++++++ .../edu/wpi/first/wpilibj/snippets/snippets.json | 12 ++++++++++++ 2 files changed, 23 insertions(+) diff --git a/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json index db114169f0f..797749be2ee 100644 --- a/wpilibcExamples/src/main/cpp/snippets/snippets.json +++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json @@ -154,5 +154,16 @@ ], "foldername": "AccelerometerFilter", "gradlebase": "cpp" + }, + { + "name": "ProfiledPIDFeedforward", + "description": "Snippets of ProfiledPIDController with feedforward for frc-docs.", + "tags": [ + "Controller", + "PID", + "Feedforward" + ], + "foldername": "ProfiledPIDFeedforward", + "gradlebase": "cpp" } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json index f3f98a47526..022ddbbeace 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json @@ -169,5 +169,17 @@ "foldername": "accelerometerfilter", "gradlebase": "java", "mainclass": "Main" + }, + { + "name": "ProfiledPIDFeedforward", + "description": "Snippets of ProfiledPIDController with feedforward for frc-docs.", + "tags": [ + "Controller", + "PID", + "Feedforward" + ], + "foldername": "profiledpidfeedforward", + "gradlebase": "java", + "mainclass": "Robot" } ] From 15a027ee8589ccb5e47c72ed08b3455af0323b19 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 7 Oct 2025 22:40:54 -0500 Subject: [PATCH 4/9] Add Main.java for ProfiledPIDFeedforward Java snippet --- .../snippets/profiledpidfeedforward/Main.java | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Main.java diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Main.java new file mode 100644 index 00000000000..1fc6c2ec2ba --- /dev/null +++ b/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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} From c78291063e1b0cab86816bf095da91d8001a9b8d Mon Sep 17 00:00:00 2001 From: jasondaming Date: Tue, 7 Oct 2025 23:00:10 -0500 Subject: [PATCH 5/9] Use valid example tags for ProfiledPIDFeedforward snippets --- wpilibcExamples/src/main/cpp/snippets/snippets.json | 3 +-- .../src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json index 797749be2ee..4e2fca80a0a 100644 --- a/wpilibcExamples/src/main/cpp/snippets/snippets.json +++ b/wpilibcExamples/src/main/cpp/snippets/snippets.json @@ -159,9 +159,8 @@ "name": "ProfiledPIDFeedforward", "description": "Snippets of ProfiledPIDController with feedforward for frc-docs.", "tags": [ - "Controller", "PID", - "Feedforward" + "Profiled PID" ], "foldername": "ProfiledPIDFeedforward", "gradlebase": "cpp" diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json index 022ddbbeace..bef8e76f4f2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/snippets.json @@ -174,9 +174,8 @@ "name": "ProfiledPIDFeedforward", "description": "Snippets of ProfiledPIDController with feedforward for frc-docs.", "tags": [ - "Controller", "PID", - "Feedforward" + "Profiled PID" ], "foldername": "profiledpidfeedforward", "gradlebase": "java", From 63a9e52e24e7d0a5a36a01645386a3a68a7a7087 Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 8 Oct 2025 13:52:59 -0500 Subject: [PATCH 6/9] Fix C++ voltage type conversion in ProfiledPIDFeedforward snippet --- .../src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp index dda857a2c4d..fc076263f64 100644 --- a/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/snippets/ProfiledPIDFeedforward/cpp/Robot.cpp @@ -25,7 +25,7 @@ class Robot : public frc::TimedRobot { void GoToPosition(units::meter_t goalPosition) { auto pidVal = m_controller.Calculate( units::meter_t{m_encoder.GetDistance()}, goalPosition); - m_motor.SetVoltage(pidVal + + m_motor.SetVoltage(units::volt_t{pidVal} + m_feedforward.Calculate( m_lastSpeed, m_controller.GetSetpoint().velocity)); m_lastSpeed = m_controller.GetSetpoint().velocity; From c66473c032790d06109318beac1509cecd1b2cbd Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 8 Oct 2025 16:33:50 -0500 Subject: [PATCH 7/9] Use calculateWithVelocities instead of deprecated calculate(velocity, acceleration) Per the SimpleMotorFeedforward API, the two-parameter calculate method that takes velocity and acceleration is deprecated. This updates the snippet to use calculateWithVelocities(currentVelocity, nextVelocity) which is the recommended approach. Addresses #8280 --- .../first/wpilibj/snippets/profiledpidfeedforward/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java index 789d476ebc4..e8253b8fb1a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java @@ -34,7 +34,7 @@ public Robot() { public void goToPosition(double goalPosition) { double pidVal = m_controller.calculate(m_encoder.getDistance(), goalPosition); m_motor.setVoltage( - pidVal + m_feedforward.calculate(m_lastSpeed, m_controller.getSetpoint().velocity)); + pidVal + m_feedforward.calculateWithVelocities(m_lastSpeed, m_controller.getSetpoint().velocity)); m_lastSpeed = m_controller.getSetpoint().velocity; } From b73cd0935f8b5cfe67f11ddfa442b22486808e6e Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 8 Oct 2025 16:52:51 -0500 Subject: [PATCH 8/9] Fix checkstyle and PMD violations in ProfiledPIDFeedforward snippet - Add Javadoc comment for goToPosition method - Split long line to stay under 100 character limit - Remove redundant field initializer for m_lastSpeed --- .../snippets/profiledpidfeedforward/Robot.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java index e8253b8fb1a..15a18e18162 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java @@ -22,19 +22,25 @@ public class Robot extends TimedRobot { private final Encoder m_encoder = new Encoder(0, 1); private final PWMSparkMax m_motor = new PWMSparkMax(0); - double m_lastSpeed = 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 + /** + * 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)); + pidVal + + m_feedforward.calculateWithVelocities( + m_lastSpeed, m_controller.getSetpoint().velocity)); m_lastSpeed = m_controller.getSetpoint().velocity; } From ed88b370355203db420e3697449d40fc94cbc52f Mon Sep 17 00:00:00 2001 From: jasondaming Date: Wed, 8 Oct 2025 20:59:27 -0500 Subject: [PATCH 9/9] Fix Javadoc formatting - keep summary on single line --- .../first/wpilibj/snippets/profiledpidfeedforward/Robot.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java index 15a18e18162..9f30e73e5f9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/snippets/profiledpidfeedforward/Robot.java @@ -30,8 +30,7 @@ public Robot() { } /** - * Controls a simple motor's position using a SimpleMotorFeedforward and a - * ProfiledPIDController. + * Controls a simple motor's position using a SimpleMotorFeedforward and a ProfiledPIDController. * * @param goalPosition the desired position */