diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp new file mode 100644 index 0000000000..c0cc19b663 --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ProfiledPIDCommand.h" + +using namespace frc2; +using State = frc::TrapezoidProfile::State; + +ProfiledPIDCommand::ProfiledPIDCommand( + frc::ProfiledPIDController controller, + std::function measurementSource, + std::function goalSource, + std::function useOutput, + std::initializer_list requirements) + : m_controller{controller}, + m_measurement{std::move(measurementSource)}, + m_goal{std::move(goalSource)}, + m_useOutput{std::move(useOutput)} { + AddRequirements(requirements); +} + +ProfiledPIDCommand::ProfiledPIDCommand( + frc::ProfiledPIDController controller, + std::function measurementSource, + std::function goalSource, + std::function useOutput, + std::initializer_list requirements) + : ProfiledPIDCommand(controller, measurementSource, + [&goalSource]() { + return State{goalSource(), 0_mps}; + }, + useOutput, requirements) {} + +ProfiledPIDCommand::ProfiledPIDCommand( + frc::ProfiledPIDController controller, + std::function measurementSource, State goal, + std::function useOutput, + std::initializer_list requirements) + : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; }, + useOutput, requirements) {} + +ProfiledPIDCommand::ProfiledPIDCommand( + frc::ProfiledPIDController controller, + std::function measurementSource, units::meter_t goal, + std::function useOutput, + std::initializer_list requirements) + : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; }, + useOutput, requirements) {} + +void ProfiledPIDCommand::Initialize() { m_controller.Reset(); } + +void ProfiledPIDCommand::Execute() { + UseOutput(m_controller.Calculate(GetMeasurement(), m_goal()), + m_controller.GetSetpoint()); +} + +void ProfiledPIDCommand::End(bool interrupted) { + UseOutput(0, State{0_m, 0_mps}); +} + +units::meter_t ProfiledPIDCommand::GetMeasurement() { return m_measurement(); } + +void ProfiledPIDCommand::UseOutput(double output, State state) { + m_useOutput(output, state); +} + +frc::ProfiledPIDController& ProfiledPIDCommand::GetController() { + return m_controller; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp new file mode 100644 index 0000000000..469e153d8c --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/ProfiledPIDSubsystem.h" + +using namespace frc2; +using State = frc::TrapezoidProfile::State; + +ProfiledPIDSubsystem::ProfiledPIDSubsystem( + frc::ProfiledPIDController controller) + : m_controller{controller} {} + +void ProfiledPIDSubsystem::Periodic() { + if (m_enabled) { + UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()), + m_controller.GetSetpoint()); + } +} + +void ProfiledPIDSubsystem::Enable() { + m_controller.Reset(); + m_enabled = true; +} + +void ProfiledPIDSubsystem::Disable() { + UseOutput(0, State{0_m, 0_mps}); + m_enabled = false; +} + +frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() { + return m_controller; +} diff --git a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp new file mode 100644 index 0000000000..7dccd2716b --- /dev/null +++ b/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc2/command/TrapezoidProfileCommand.h" + +#include + +using namespace frc2; + +TrapezoidProfileCommand::TrapezoidProfileCommand( + frc::TrapezoidProfile profile, + std::function output, + std::initializer_list requirements) + : m_profile(profile), m_output(output) { + AddRequirements(requirements); +} + +void TrapezoidProfileCommand::Initialize() { + m_timer.Reset(); + m_timer.Start(); +} + +void TrapezoidProfileCommand::Execute() { + m_output(m_profile.Calculate(units::second_t(m_timer.Get()))); +} + +void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); } + +bool TrapezoidProfileCommand::IsFinished() { + return m_timer.HasPeriodPassed(m_profile.TotalTime()); +} diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 6e0bf60316..c528c9e7fd 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -68,6 +68,8 @@ class TrapezoidProfile { TrapezoidProfile(Constraints constraints, State goal, State initial = State{0_m, 0_mps}); + TrapezoidProfile(const TrapezoidProfile&) = default; + TrapezoidProfile& operator=(const TrapezoidProfile&) = default; TrapezoidProfile(TrapezoidProfile&&) = default; TrapezoidProfile& operator=(TrapezoidProfile&&) = default; diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h new file mode 100644 index 0000000000..b1ff9e6524 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h @@ -0,0 +1,132 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/controller/ProfiledPIDController.h" +#include "frc2/command/CommandBase.h" +#include "frc2/command/CommandHelper.h" + +namespace frc2 { +/** + * A command that controls an output with a ProfiledPIDController. Runs forever + * by default - to add exit conditions and/or other behavior, subclass this + * class. The controller calculation and output are performed synchronously in + * the command's execute() method. + * + * @see ProfiledPIDController + */ +class ProfiledPIDCommand + : public CommandHelper { + using State = frc::TrapezoidProfile::State; + + public: + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goalSource the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + std::function goalSource, + std::function useOutput, + std::initializer_list requirements); + + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goalSource the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + std::function goalSource, + std::function useOutput, + std::initializer_list requirements); + + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController with a constant goal. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goal the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + State goal, std::function useOutput, + std::initializer_list requirements); + + /** + * Creates a new PIDCommand, which controls the given output with a + * ProfiledPIDController with a constant goal. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goal the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + ProfiledPIDCommand(frc::ProfiledPIDController controller, + std::function measurementSource, + units::meter_t goal, + std::function useOutput, + std::initializer_list requirements); + + ProfiledPIDCommand(ProfiledPIDCommand&& other) = default; + + ProfiledPIDCommand(const ProfiledPIDCommand& other) = default; + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + /** + * Gets the measurement of the process variable. Wraps the passed-in function + * for readability. + * + * @return The measurement of the process variable + */ + virtual units::meter_t GetMeasurement(); + + /** + * Gets the measurement of the process variable. Wraps the passed-in function + * for readability. + * + * @return The measurement of the process variable + */ + virtual void UseOutput(double output, State state); + + /** + * Returns the ProfiledPIDController used by the command. + * + * @return The ProfiledPIDController + */ + frc::ProfiledPIDController& GetController(); + + protected: + frc::ProfiledPIDController m_controller; + std::function m_measurement; + std::function m_goal; + std::function m_useOutput; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h new file mode 100644 index 0000000000..4fa47b247d --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "frc/controller/ProfiledPIDController.h" +#include "frc2/command/SubsystemBase.h" + +namespace frc2 { +/** + * A subsystem that uses a ProfiledPIDController to control an output. The + * controller is run synchronously from the subsystem's periodic() method. + * + * @see ProfiledPIDController + */ +class ProfiledPIDSubsystem : public SubsystemBase { + using State = frc::TrapezoidProfile::State; + + public: + /** + * Creates a new ProfiledPIDSubsystem. + * + * @param controller the ProfiledPIDController to use + */ + explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller); + + void Periodic() override; + + /** + * Uses the output from the ProfiledPIDController. + * + * @param output the output of the ProfiledPIDController + */ + virtual void UseOutput(double output, State state) = 0; + + /** + * Returns the goal used by the ProfiledPIDController. + * + * @return the goal to be used by the controller + */ + virtual State GetGoal() = 0; + + /** + * Returns the measurement of the process variable used by the + * ProfiledPIDController. + * + * @return the measurement of the process variable + */ + virtual units::meter_t GetMeasurement() = 0; + + /** + * Enables the PID control. Resets the controller. + */ + virtual void Enable(); + + /** + * Disables the PID control. Sets output to zero. + */ + virtual void Disable(); + + /** + * Returns the ProfiledPIDController. + * + * @return The controller. + */ + frc::ProfiledPIDController& GetController(); + + protected: + frc::ProfiledPIDController m_controller; + bool m_enabled; +}; +} // namespace frc2 diff --git a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h new file mode 100644 index 0000000000..d2074813b1 --- /dev/null +++ b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include + +#include "CommandBase.h" +#include "CommandHelper.h" + +#pragma once + +namespace frc2 { +/** + * A command that runs a TrapezoidProfile. Useful for smoothly controlling + * mechanism motion. + * + * @see TrapezoidProfile + */ +class TrapezoidProfileCommand + : public CommandHelper { + public: + /** + * Creates a new TrapezoidProfileCommand that will execute the given + * TrapezoidalProfile. Output will be piped to the provided consumer function. + * + * @param profile The motion profile to execute. + * @param output The consumer for the profile output. + */ + TrapezoidProfileCommand( + frc::TrapezoidProfile profile, + std::function output, + std::initializer_list requirements); + + void Initialize() override; + + void Execute() override; + + void End(bool interrupted) override; + + bool IsFinished() override; + + private: + frc::TrapezoidProfile m_profile; + std::function m_output; + + frc::Timer m_timer; +}; + +} // namespace frc2 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java index 4e71f1cc74..8bd87673a4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java @@ -80,15 +80,6 @@ public class PIDCommand extends CommandBase { useOutput(0); } - /** - * Sets the function that uses the output of the PIDController. - * - * @param useOutput The function that uses the output. - */ - public final void setOutput(DoubleConsumer useOutput) { - m_useOutput = useOutput; - } - /** * Returns the PIDController used by the command. * @@ -98,34 +89,6 @@ public class PIDCommand extends CommandBase { return m_controller; } - /** - * Sets the setpoint for the controller to track the given source. - * - * @param setpointSource The setpoint source - */ - public void setSetpoint(DoubleSupplier setpointSource) { - m_setpoint = setpointSource; - } - - /** - * Sets the setpoint for the controller to a constant value. - * - * @param setpoint The setpoint - */ - public void setSetpoint(double setpoint) { - setSetpoint(() -> setpoint); - } - - /** - * Sets the setpoint for the controller to a constant value relative (i.e. added to) the current - * setpoint. - * - * @param relativeReference The change in setpoint - */ - public void setSetpointRelative(double relativeReference) { - setSetpoint(m_controller.getSetpoint() + relativeReference); - } - /** * Gets the setpoint for the controller. Wraps the passed-in function for readability. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java new file mode 100644 index 0000000000..fcdb36bbe3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java @@ -0,0 +1,164 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Set; +import java.util.function.BiConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; + +import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State; +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that controls an output with a {@link ProfiledPIDController}. Runs forever by + * default - to add + * exit conditions and/or other behavior, subclass this class. The controller calculation and + * output are performed synchronously in the command's execute() method. + */ +public class ProfiledPIDCommand extends CommandBase { + protected final ProfiledPIDController m_controller; + protected DoubleSupplier m_measurement; + protected Supplier m_goal; + protected BiConsumer m_useOutput; + + /** + * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. + * Goal velocity is specified. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goalSource the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource, + Supplier goalSource, BiConsumer useOutput, + Subsystem... requirements) { + requireNonNullParam(controller, "controller", "SynchronousPIDCommand"); + requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand"); + requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand"); + requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand"); + + m_controller = controller; + m_useOutput = useOutput; + m_measurement = measurementSource; + m_goal = goalSource; + m_requirements.addAll(Set.of(requirements)); + } + + /** + * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. + * Goal velocity is implicitly zero. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goalSource the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource, + DoubleSupplier goalSource, BiConsumer useOutput, + Subsystem... requirements) { + requireNonNullParam(controller, "controller", "SynchronousPIDCommand"); + requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand"); + requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand"); + requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand"); + + m_controller = controller; + m_useOutput = useOutput; + m_measurement = measurementSource; + m_goal = () -> new State(goalSource.getAsDouble(), 0); + m_requirements.addAll(Set.of(requirements)); + } + + /** + * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal + * velocity is specified. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goal the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource, + State goal, BiConsumer useOutput, + Subsystem... requirements) { + this(controller, measurementSource, () -> goal, useOutput, requirements); + } + + /** + * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal + * velocity is implicitly zero. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param goal the controller's goal + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource, + double goal, BiConsumer useOutput, + Subsystem... requirements) { + this(controller, measurementSource, () -> goal, useOutput, requirements); + } + + @Override + public void initialize() { + m_controller.reset(); + } + + @Override + public void execute() { + useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint()); + } + + @Override + public void end(boolean interrupted) { + useOutput(0, new State()); + } + + /** + * Returns the ProfiledPIDController used by the command. + * + * @return The ProfiledPIDController + */ + public ProfiledPIDController getController() { + return m_controller; + } + + /** + * Gets the goal for the controller. Wraps the passed-in function for readability. + * + * @return The goal for the controller + */ + private State getGoal() { + return m_goal.get(); + } + + /** + * Gets the measurement of the process variable. Wraps the passed-in function for readability. + * + * @return The measurement of the process variable + */ + private double getMeasurement() { + return m_measurement.getAsDouble(); + } + + /** + * Uses the output of the controller. Wraps the passed-in function for readability. + * + * @param output The output value to use + */ + private void useOutput(double output, State state) { + m_useOutput.accept(output, state); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java new file mode 100644 index 0000000000..ecd33719cc --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java @@ -0,0 +1,81 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import edu.wpi.first.wpilibj.controller.ProfiledPIDController; + +import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State; +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is + * run synchronously from the subsystem's periodic() method. + */ +public abstract class ProfiledPIDSubsystem extends SubsystemBase { + protected final ProfiledPIDController m_controller; + protected boolean m_enabled; + + /** + * Creates a new ProfiledPIDSubsystem. + * + * @param controller the ProfiledPIDController to use + */ + public ProfiledPIDSubsystem(ProfiledPIDController controller) { + requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem"); + m_controller = controller; + } + + @Override + public void periodic() { + if (m_enabled) { + useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint()); + } + } + + public ProfiledPIDController getController() { + return m_controller; + } + + /** + * Uses the output from the ProfiledPIDController. + * + * @param output the output of the ProfiledPIDController + * @param goal the goal state of the ProfiledPIDController, for feedforward + */ + public abstract void useOutput(double output, State goal); + + /** + * Returns the goal used by the ProfiledPIDController. + * + * @return the goal to be used by the controller + */ + public abstract State getGoal(); + + /** + * Returns the measurement of the process variable used by the ProfiledPIDController. + * + * @return the measurement of the process variable + */ + public abstract double getMeasurement(); + + /** + * Enables the PID control. Resets the controller. + */ + public void enable() { + m_enabled = true; + m_controller.reset(); + } + + /** + * Disables the PID control. Sets output to zero. + */ + public void disable() { + m_enabled = false; + useOutput(0, new State()); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java new file mode 100644 index 0000000000..85ddd03f76 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java @@ -0,0 +1,64 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.Consumer; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State; +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism + * motion. + */ +public class TrapezoidProfileCommand extends CommandBase { + private final TrapezoidProfile m_profile; + private final Consumer m_output; + + private final Timer m_timer = new Timer(); + + /** + * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}. + * Output will be piped to the provided consumer function. + * + * @param profile The motion profile to execute. + * @param output The consumer for the profile output. + * @param requirements The subsystems required by this command. + */ + public TrapezoidProfileCommand(TrapezoidProfile profile, + Consumer output, + Subsystem... requirements) { + m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand"); + m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand"); + addRequirements(requirements); + } + + @Override + public void initialize() { + m_timer.reset(); + m_timer.start(); + } + + @Override + public void execute() { + m_output.accept(m_profile.calculate(m_timer.get())); + } + + @Override + public void end(boolean interrupted) { + m_timer.stop(); + } + + @Override + public boolean isFinished() { + return m_timer.hasPeriodPassed(m_profile.totalTime()); + } +}