diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp index 4391e606d8..d3e0c0c6ab 100644 --- a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp +++ b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp @@ -31,29 +31,9 @@ PIDCommand::PIDCommand(PIDController controller, void PIDCommand::Initialize() { m_controller.Reset(); } void PIDCommand::Execute() { - UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint())); + m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint())); } -void PIDCommand::End(bool interrupted) { UseOutput(0); } - -void PIDCommand::SetOutput(std::function useOutput) { - m_useOutput = useOutput; -} - -void PIDCommand::SetSetpoint(std::function setpointSource) { - m_setpoint = setpointSource; -} - -void PIDCommand::SetSetpoint(double setpoint) { - m_setpoint = [setpoint] { return setpoint; }; -} - -void PIDCommand::SetSetpointRelative(double relativeSetpoint) { - SetSetpoint(m_setpoint() + relativeSetpoint); -} - -double PIDCommand::GetMeasurement() { return m_measurement(); } - -void PIDCommand::UseOutput(double output) { m_useOutput(output); } +void PIDCommand::End(bool interrupted) { m_useOutput(0); } PIDController& PIDCommand::getController() { return m_controller; } diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp index c0cc19b663..e976702ed9 100644 --- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp +++ b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp @@ -54,18 +54,12 @@ ProfiledPIDCommand::ProfiledPIDCommand( void ProfiledPIDCommand::Initialize() { m_controller.Reset(); } void ProfiledPIDCommand::Execute() { - UseOutput(m_controller.Calculate(GetMeasurement(), m_goal()), - m_controller.GetSetpoint()); + m_useOutput(m_controller.Calculate(m_measurement(), 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); + m_useOutput(0, State{0_m, 0_mps}); } frc::ProfiledPIDController& ProfiledPIDCommand::GetController() { diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h index 6e553ff39e..4a13f3dd3c 100644 --- a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h +++ b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h @@ -63,51 +63,6 @@ class PIDCommand : public CommandHelper { void End(bool interrupted) override; - /** - * Sets the function that uses the output of the PIDController. - * - * @param useOutput The function that uses the output. - */ - void SetOutput(std::function useOutput); - - /** - * Sets the setpoint for the controller to track the given source. - * - * @param setpointSource The setpoint source - */ - void SetSetpoint(std::function setpointSource); - - /** - * Sets the setpoint for the controller to a constant value. - * - * @param setpoint The setpoint - */ - void SetSetpoint(double 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 - */ - void SetSetpointRelative(double relativeSetpoint); - - /** - * Gets the measurement of the process variable. Wraps the passed-in function - * for readability. - * - * @return The measurement of the process variable - */ - virtual double 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); - /** * Returns the PIDController used by the command. * diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h index b1ff9e6524..4e46e5395e 100644 --- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h +++ b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h @@ -100,22 +100,6 @@ class ProfiledPIDCommand 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. * 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 8bd87673a4..5f2b093b7d 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 @@ -72,12 +72,13 @@ public class PIDCommand extends CommandBase { @Override public void execute() { - useOutput(m_controller.calculate(getMeasurement(), getSetpoint())); + m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), + m_setpoint.getAsDouble())); } @Override public void end(boolean interrupted) { - useOutput(0); + m_useOutput.accept(0); } /** @@ -88,31 +89,4 @@ public class PIDCommand extends CommandBase { public PIDController getController() { return m_controller; } - - /** - * Gets the setpoint for the controller. Wraps the passed-in function for readability. - * - * @return The setpoint for the controller - */ - private double getSetpoint() { - return m_setpoint.getAsDouble(); - } - - /** - * 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) { - m_useOutput.accept(output); - } } 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 index fcdb36bbe3..733b37bf95 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java @@ -118,12 +118,13 @@ public class ProfiledPIDCommand extends CommandBase { @Override public void execute() { - useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint()); + m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()), + m_controller.getSetpoint()); } @Override public void end(boolean interrupted) { - useOutput(0, new State()); + m_useOutput.accept(0., new State()); } /** @@ -134,31 +135,4 @@ public class ProfiledPIDCommand extends CommandBase { 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); - } }