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 0000000000..fc076263f6 --- /dev/null +++ b/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 +#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(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 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/wpilibcExamples/src/main/cpp/snippets/snippets.json b/wpilibcExamples/src/main/cpp/snippets/snippets.json index db114169f0..4e2fca80a0 100644 --- a/wpilibcExamples/src/main/cpp/snippets/snippets.json +++ b/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" } ] 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 0000000000..1fc6c2ec2b --- /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); + } +} 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 0000000000..9f30e73e5f --- /dev/null +++ b/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); + } +} 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 f3f98a4752..bef8e76f4f 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,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" } ]