diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java index 575ebb4348..f9337c6052 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java @@ -25,10 +25,20 @@ public abstract class PIDSubsystem extends SubsystemBase { * Creates a new PIDSubsystem. * * @param controller the PIDController to use + * @param initialPosition the initial setpoint of the subsystem + */ + public PIDSubsystem(PIDController controller, double initialPosition) { + setSetpoint(initialPosition); + m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem"); + } + + /** + * Creates a new PIDSubsystem. Initial setpoint is zero. + * + * @param controller the PIDController to use */ public PIDSubsystem(PIDController controller) { - requireNonNullParam(controller, "controller", "PIDSubsystem"); - m_controller = controller; + this(controller, 0); } @Override diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java index 6b3492d8ed..07283c504a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java @@ -27,10 +27,21 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase { * Creates a new ProfiledPIDSubsystem. * * @param controller the ProfiledPIDController to use + * @param initialPosition the initial goal position of the controller + */ + public ProfiledPIDSubsystem(ProfiledPIDController controller, + double initialPosition) { + m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem"); + setGoal(initialPosition); + } + + /** + * Creates a new ProfiledPIDSubsystem. Initial goal position is zero. + * + * @param controller the ProfiledPIDController to use */ public ProfiledPIDSubsystem(ProfiledPIDController controller) { - requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem"); - m_controller = controller; + this(controller, 0); } @Override diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java index b7ec8c1b57..05e63863b9 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java @@ -9,6 +9,8 @@ package edu.wpi.first.wpilibj2.command; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + /** * A subsystem that generates and runs trapezoidal motion profiles automatically. The user * specifies how to use the current state of the motion profile by overriding the `useState` method. @@ -20,41 +22,53 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase { private TrapezoidProfile.State m_state; private TrapezoidProfile.State m_goal; - /** - * Creates a new TrapezoidProfileSubsystem. - * - * @param constraints The constraints (maximum velocity and acceleration) for the profiles. - * @param initialPosition The initial position of the controller mechanism when the subsystem - * is constructed. - */ - public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints, - double initialPosition) { - m_constraints = constraints; - m_state = new TrapezoidProfile.State(initialPosition, 0); - m_period = 0.02; - } + private boolean m_enabled = true; /** * Creates a new TrapezoidProfileSubsystem. * * @param constraints The constraints (maximum velocity and acceleration) for the profiles. - * @param initialPosition The initial position of the controller mechanism when the subsystem + * @param initialPosition The initial position of the controlled mechanism when the subsystem * is constructed. * @param period The period of the main robot loop, in seconds. */ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints, double initialPosition, double period) { - m_constraints = constraints; + m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem"); m_state = new TrapezoidProfile.State(initialPosition, 0); + setGoal(initialPosition); m_period = period; } + /** + * Creates a new TrapezoidProfileSubsystem. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + * @param initialPosition The initial position of the controlled mechanism when the subsystem + * is constructed. + */ + public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints, + double initialPosition) { + this(constraints, initialPosition, 0.02); + } + + /** + * Creates a new TrapezoidProfileSubsystem. + * + * @param constraints The constraints (maximum velocity and acceleration) for the profiles. + */ + public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) { + this(constraints, 0, 0.02); + } + @Override public void periodic() { var profile = new TrapezoidProfile(m_constraints, m_goal, m_state); m_state = profile.calculate(m_period); - useState(m_state); + if (m_enabled) { + useState(m_state); + } } /** @@ -75,6 +89,20 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase { setGoal(new TrapezoidProfile.State(goal, 0)); } + /** + * Enable the TrapezoidProfileSubsystem's output. + */ + public void enable() { + m_enabled = true; + } + + /** + * Disable the TrapezoidProfileSubsystem's output. + */ + public void disable() { + m_enabled = false; + } + /** * Users should override this to consume the current state of the motion profile. * diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp index ce8198af62..14d965ce76 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp @@ -9,8 +9,10 @@ using namespace frc2; -PIDSubsystem::PIDSubsystem(PIDController controller) - : m_controller{controller} {} +PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition) + : m_controller{controller} { + SetSetpoint(initialPosition); +} void PIDSubsystem::Periodic() { if (m_enabled) { diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h index 62389c30e5..3a6df88c1e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h @@ -24,8 +24,9 @@ class PIDSubsystem : public SubsystemBase { * Creates a new PIDSubsystem. * * @param controller the PIDController to use + * @param initialPosition the initial setpoint of the subsystem */ - explicit PIDSubsystem(PIDController controller); + explicit PIDSubsystem(PIDController controller, double initialPosition = 0); void Periodic() override; @@ -62,7 +63,7 @@ class PIDSubsystem : public SubsystemBase { protected: PIDController m_controller; - bool m_enabled; + bool m_enabled{false}; /** * Returns the measurement of the process variable used by the PIDController. diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h index d5302c5988..8d1a618e56 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h @@ -32,9 +32,13 @@ class ProfiledPIDSubsystem : public SubsystemBase { * Creates a new ProfiledPIDSubsystem. * * @param controller the ProfiledPIDController to use + * @param initialPosition the initial goal position of the subsystem */ - explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller) - : m_controller{controller} {} + explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller, + Distance_t initialPosition = Distance_t{0}) + : m_controller{controller} { + SetGoal(initialPosition); + } void Periodic() override { if (m_enabled) { diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index e21de074dc..6e04e9b96c 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -42,7 +42,7 @@ class TrapezoidProfileCommand */ TrapezoidProfileCommand(frc::TrapezoidProfile profile, std::function output, - std::initializer_list requirements) + std::initializer_list requirements = {}) : m_profile(profile), m_output(output) { this->AddRequirements(requirements); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h index a833f3ca2a..c5d7e359b2 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h @@ -37,18 +37,21 @@ class TrapezoidProfileSubsystem : public SubsystemBase { * when the subsystem is constructed. * @param period The period of the main robot loop, in seconds. */ - TrapezoidProfileSubsystem(Constraints constraints, Distance_t position, - units::second_t period = 20_ms) + explicit TrapezoidProfileSubsystem(Constraints constraints, + Distance_t initialPosition = Distance_t{0}, + units::second_t period = 20_ms) : m_constraints(constraints), - m_state{position, Velocity_t(0)}, - m_goal{position, Velocity_t{0}}, + m_state{initialPosition, Velocity_t(0)}, + m_goal{initialPosition, Velocity_t{0}}, m_period(period) {} void Periodic() override { auto profile = frc::TrapezoidProfile(m_constraints, m_goal, m_state); m_state = profile.Calculate(m_period); - UseState(m_state); + if (m_enabled) { + UseState(m_state); + } } /** @@ -74,10 +77,21 @@ class TrapezoidProfileSubsystem : public SubsystemBase { */ virtual void UseState(State state) = 0; + /** + * Enable the TrapezoidProfileSubsystem's output. + */ + void Enable() { m_enabled = true; } + + /** + * Disable the TrapezoidProfileSubsystem's output. + */ + void Disable() { m_enabled = false; } + private: Constraints m_constraints; State m_state; State m_goal; units::second_t m_period; + bool m_enabled{false}; }; } // namespace frc2 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java index 7d549e57f3..85aafeade2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java @@ -39,12 +39,11 @@ public class ArmSubsystem extends ProfiledPIDSubsystem { * Create a new ArmSubsystem. */ public ArmSubsystem() { - super(new ProfiledPIDController( - kP, - 0, - 0, - new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond, - kMaxAccelerationRadPerSecSquared))); + super(new ProfiledPIDController(kP, 0, 0, + new TrapezoidProfile.Constraints( + kMaxVelocityRadPerSecond, + kMaxAccelerationRadPerSecSquared)), + 0); m_encoder.setDistancePerPulse(kEncoderDistancePerPulse); // Start arm at rest in neutral position setGoal(kArmOffsetRads);