From 5c4cc30d5fb87234f4e6f426d9b448552013bbae Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Fri, 25 Jul 2025 12:56:33 -0700 Subject: [PATCH 1/6] Improvements to MotorSubsystem - Add `setSetpointCommand()` - Add `clampOutput()` protected method - Add `useOutput(double)` - Deprecate `useOutput(double, double)` - Remove `setpoint` field (instead, get from the controller) - Improve Javadoc --- .../lib2813/subsystems/MotorSubsystem.java | 93 +++++++++++++++---- 1 file changed, 73 insertions(+), 20 deletions(-) diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index e4e39094..7c4ff121 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -10,6 +10,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Objects; import java.util.function.Supplier; @@ -29,11 +31,9 @@ public abstract class MotorSubsystem> extends Subsyste protected final double acceptableError; protected final PIDController controller; - private double setpoint; private boolean isEnabled; protected MotorSubsystem(MotorSubsystemConfiguration builder) { - this.setpoint = builder.startingPosition; this.controller = builder.controller; this.controller.setTolerance(builder.acceptableError); acceptableError = builder.acceptableError; @@ -41,27 +41,40 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) { encoder = builder.encoder; controlMode = builder.controlMode; rotationUnit = builder.rotationUnit; + + this.controller.setSetpoint(builder.startingPosition); } /** - * Sets the desired setpoint to the current setpoint, and enables the PID. control. + * Sets the desired setpoint to the provided value, and enables the PID control. * - * @param setpoint the position to go to + * @param position the position to go to. */ - public void setSetpoint(T setpoint) { + public void setSetpoint(T position) { if (!isEnabled()) { enable(); } - this.setpoint = setpoint.get().in(rotationUnit); - controller.setSetpoint(this.setpoint); + double setpoint = position.get().in(rotationUnit); + controller.setSetpoint(setpoint); } + /** + * Returns a command that sets the desired setpoint to the provided value. + * + * @param setpoint the position to go to. + */ + public final Command setSetpointCommand(T setpoint) { + return new InstantCommand(() -> this.setSetpoint(setpoint), this); + } + + /** Returns the current setpoint as an angle. */ public Angle getSetpoint() { - return rotationUnit.of(setpoint); + return rotationUnit.of(controller.getSetpoint()); } + /** Determines if the motor is at the current setpoint, within the acceptable error. */ public boolean atPosition() { - return Math.abs(getMeasurement() - setpoint) <= acceptableError; + return Math.abs(getMeasurement() - controller.getSetpoint()) <= acceptableError; } /** @@ -103,18 +116,50 @@ public boolean isEnabled() { /** * {@inheritDoc} * - *

Additionally, this method disables PID control of the subsystem + *

Additionally, this method disables PID control of the subsystem. It does not clamp + * the provided value. */ @Override public void set(ControlMode mode, double demand) { - if (isEnabled()) { - disable(); - } + isEnabled = false; motor.set(mode, demand); } + /** + * Clamps the given output value and provides it to the motor. + * + *

This was protected and non-final to allow subclasses to clamp the output. Subclasses should + * override {@link #clampOutput(double)}. + * + * @param output The output calculated by the PID algorithm. + * @param setpoint Ignored. + * @deprecated Subclasses should override {@link #clampOutput(double)}. + */ + @Deprecated protected void useOutput(double output, double setpoint) { - motor.set(controlMode, output); + motor.set(controlMode, clampOutput(output)); + } + + /** + * Clamps the given output value and provides it to the motor. + * + *

This is called by {@link #periodic()} if this subsystem is enabled. + */ + private void useOutput(double output) { + useOutput(output, controller.getSetpoint()); + } + + /** + * Extension point that allows subclasses to clamp the output. + * + *

The default implementation returns the provided value. + * + * @param output Output provided by the PID controller. + * @return Output to provide to the motor. + * @see edu.wpi.first.math.MathUtil#clamp(double, double, double) + */ + protected double clampOutput(double output) { + return output; } protected double getMeasurement() { @@ -154,7 +199,7 @@ public AngularVelocity getVelocityMeasure() { @Override public void periodic() { if (isEnabled) { - useOutput(controller.calculate(getMeasurement()), setpoint); + useOutput(controller.calculate(getMeasurement())); } } @@ -240,9 +285,9 @@ public MotorSubsystemConfiguration PID(double p, double i, double d) { } /** - * sets the starting position. + * Sets the initial setpoint of the controller * - * @param startingPosition the position to start at + * @param startingPosition the initial setpoint * @return {@code this} for chaining */ public MotorSubsystemConfiguration startingPosition(Angle startingPosition) { @@ -250,9 +295,17 @@ public MotorSubsystemConfiguration startingPosition(Angle startingPosition) { return this; } - public MotorSubsystemConfiguration startingPosition(Supplier startingPosition) { - this.startingPosition = startingPosition.get().in(this.rotationUnit); - return this; + /** + * Sets the initial setpoint of the controller from the current value of a supplier. + * + *

This is provided to allow the subclass to define an {@code Enum} (that implements {@code + * Supplier}) which defines the supported positions of this subsystem. + * + * @param startingPositionSupplier supplier to use to get the initial setpoint + * @return {@code this} for chaining + */ + public MotorSubsystemConfiguration startingPosition(Supplier startingPositionSupplier) { + return startingPosition(startingPositionSupplier.get()); } public MotorSubsystemConfiguration acceptableError(double error) { From 23ca267af310e01291875f77ae1a6b0f8f856202 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 24 Aug 2025 22:18:22 -0700 Subject: [PATCH 2/6] Add @Override methods to overriden methods; update @Deprecated annotations --- .../com/team2813/lib2813/subsystems/MotorSubsystem.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index 7c4ff121..ebeb2c7b 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -167,31 +167,34 @@ protected double getMeasurement() { } @Override - @Deprecated + @Deprecated(forRemoval = true) public double position() { return encoder.position(); } + @Override public Angle getPositionMeasure() { return encoder.getPositionMeasure(); } @Override - @Deprecated + @Deprecated(forRemoval = true) public void setPosition(double position) { encoder.setPosition(position); } + @Override public void setPosition(Angle position) { encoder.setPosition(position); } @Override - @Deprecated + @Deprecated(forRemoval = true) public double getVelocity() { return encoder.getVelocity(); } + @Override public AngularVelocity getVelocityMeasure() { return encoder.getVelocityMeasure(); } From 65bb80006e25eb6b0acb8e5db1de2b675d6e0605 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 24 Aug 2025 22:46:43 -0700 Subject: [PATCH 3/6] Do not clamp output in disable() --- .../java/com/team2813/lib2813/subsystems/MotorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index ebeb2c7b..2cf579fd 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -101,7 +101,7 @@ public void enable() { public void disable() { isEnabled = false; - useOutput(0, 0); + motor.set(controlMode, 0); } /** From 6336690810b308eb5f284752dcd631c3fc15b3c9 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 21 Sep 2025 16:32:23 -0700 Subject: [PATCH 4/6] Make methods final that can be final --- .../lib2813/subsystems/MotorSubsystem.java | 38 ++++++++++++------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index 2cf579fd..f9583fef 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -50,7 +50,7 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) { * * @param position the position to go to. */ - public void setSetpoint(T position) { + public final void setSetpoint(T position) { if (!isEnabled()) { enable(); } @@ -68,12 +68,12 @@ public final Command setSetpointCommand(T setpoint) { } /** Returns the current setpoint as an angle. */ - public Angle getSetpoint() { + public final Angle getSetpoint() { return rotationUnit.of(controller.getSetpoint()); } /** Determines if the motor is at the current setpoint, within the acceptable error. */ - public boolean atPosition() { + public final boolean atPosition() { return Math.abs(getMeasurement() - controller.getSetpoint()) <= acceptableError; } @@ -83,7 +83,7 @@ public boolean atPosition() { *

Additionally, this method disables PID control of the subsystem */ @Override - public void set(ControlMode mode, double demand, double feedForward) { + public final void set(ControlMode mode, double demand, double feedForward) { if (isEnabled()) { disable(); } @@ -91,15 +91,27 @@ public void set(ControlMode mode, double demand, double feedForward) { } @Override - public Current getAppliedCurrent() { + public final Current getAppliedCurrent() { return motor.getAppliedCurrent(); } - public void enable() { + /** + * Enables the motor + * + *

The motor voltage will be periodically updated to move the motor towards the current + * setupoint. + */ + public final void enable() { isEnabled = true; } - public void disable() { + /** + * Stops the motor. + * + *

The motor voltage will be set to zero, and the motor will not adjust to move towards the + * current setpoint. + */ + public final void disable() { isEnabled = false; motor.set(controlMode, 0); } @@ -109,7 +121,7 @@ public void disable() { * * @return Whether the controller is enabled. */ - public boolean isEnabled() { + public final boolean isEnabled() { return isEnabled; } @@ -120,7 +132,7 @@ public boolean isEnabled() { * the provided value. */ @Override - public void set(ControlMode mode, double demand) { + public final void set(ControlMode mode, double demand) { isEnabled = false; motor.set(mode, demand); } @@ -162,7 +174,7 @@ protected double clampOutput(double output) { return output; } - protected double getMeasurement() { + protected final double getMeasurement() { return encoder.getPositionMeasure().in(rotationUnit); } @@ -173,7 +185,7 @@ public double position() { } @Override - public Angle getPositionMeasure() { + public final Angle getPositionMeasure() { return encoder.getPositionMeasure(); } @@ -184,7 +196,7 @@ public void setPosition(double position) { } @Override - public void setPosition(Angle position) { + public final void setPosition(Angle position) { encoder.setPosition(position); } @@ -195,7 +207,7 @@ public double getVelocity() { } @Override - public AngularVelocity getVelocityMeasure() { + public final AngularVelocity getVelocityMeasure() { return encoder.getVelocityMeasure(); } From ac89ca84a639e0c27df87827f1da1c8d7874b822 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Fri, 3 Oct 2025 11:48:30 -0700 Subject: [PATCH 5/6] Improve JavaDoc --- .../lib2813/subsystems/MotorSubsystem.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index f9583fef..288ee514 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -19,7 +19,7 @@ /** * Defines PID control over a motor, with values specified by an encoder * - * @param the {@link Supplier} type to use positions from. + * @param the type of the {@link Supplier} used to specify setpoints. */ public abstract class MotorSubsystem> extends SubsystemBase implements Motor, Encoder { @@ -211,6 +211,7 @@ public final AngularVelocity getVelocityMeasure() { return encoder.getVelocityMeasure(); } + /** Applies the PID output to the motor if this subsystem is enabled. */ @Override public void periodic() { if (isEnabled) { @@ -220,10 +221,10 @@ public void periodic() { /** A configuration for a MotorSubsystem */ public static class MotorSubsystemConfiguration { - /** The default error of a motor subsystems */ + /** The default acceptable position error. */ public static final double DEFAULT_ERROR = 5.0; - /** The default starting position if one is not defined */ + /** The default starting position if one is not provided. */ public static final double DEFAULT_STARTING_POSITION = 0.0; private ControlMode controlMode; @@ -235,12 +236,12 @@ public static class MotorSubsystemConfiguration { private double startingPosition; /** - * Creates a new config for MotorSubsystems. The default acceptable error is {@value + * Creates a new configuration for a MotorSubsystems. The default acceptable error is {@value * #DEFAULT_ERROR}, the PID constants are set to 0, and the starting position is {@value * #DEFAULT_STARTING_POSITION} * - * @param motor the motor to use - * @param encoder the encoder to use + * @param motor the motor to control + * @param encoder the encoder providing feedback */ public MotorSubsystemConfiguration(Motor motor, Encoder encoder) { this.motor = Objects.requireNonNull(motor, "motor should not be null"); @@ -257,7 +258,7 @@ public MotorSubsystemConfiguration(Motor motor, Encoder encoder) { * default acceptable error is {@value #DEFAULT_ERROR}, the PID constants are set to 0, and the * starting position is {@value #DEFAULT_STARTING_POSITION} * - * @param motor the motor to use that also supports an encoder + * @param motor the integrated motor controller */ public MotorSubsystemConfiguration(PIDMotor motor) { this(motor, motor); @@ -323,6 +324,7 @@ public MotorSubsystemConfiguration startingPosition(Supplier startingPosi return startingPosition(startingPositionSupplier.get()); } + /** Sets the acceptable position error. */ public MotorSubsystemConfiguration acceptableError(double error) { this.acceptableError = error; return this; From 056878e733fbc25514d3b825ba1c3afa84516095 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 24 Aug 2025 19:46:43 -0700 Subject: [PATCH 6/6] Update MotorSubsystem to optionally publish to NetworkTables (#47) --- .../lib2813/subsystems/MotorSubsystem.java | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index 288ee514..c8052804 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -1,10 +1,17 @@ package com.team2813.lib2813.subsystems; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Rotations; + import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.Encoder; import com.team2813.lib2813.control.Motor; import com.team2813.lib2813.control.PIDMotor; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; @@ -30,6 +37,10 @@ public abstract class MotorSubsystem> extends Subsyste protected final AngleUnit rotationUnit; protected final double acceptableError; protected final PIDController controller; + private final DoublePublisher positionPublisher; + private final BooleanPublisher atPositionPublisher; + private final DoublePublisher appliedCurrentPublisher; + private final DoublePublisher setpointPublisher; private boolean isEnabled; @@ -41,6 +52,18 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) { encoder = builder.encoder; controlMode = builder.controlMode; rotationUnit = builder.rotationUnit; + if (builder.ntInstance != null) { + NetworkTable networkTable = builder.ntInstance.getTable(getName()); + positionPublisher = networkTable.getDoubleTopic("position").publish(); + atPositionPublisher = networkTable.getBooleanTopic("at position").publish(); + appliedCurrentPublisher = networkTable.getDoubleTopic("applied current").publish(); + setpointPublisher = networkTable.getDoubleTopic("setpoint").publish(); + } else { + positionPublisher = null; + atPositionPublisher = null; + appliedCurrentPublisher = null; + setpointPublisher = null; + } this.controller.setSetpoint(builder.startingPosition); } @@ -217,6 +240,12 @@ public void periodic() { if (isEnabled) { useOutput(controller.calculate(getMeasurement())); } + if (positionPublisher != null) { + positionPublisher.set(getPositionMeasure().in(Rotations)); + setpointPublisher.set(getSetpoint().in(Rotations)); + atPositionPublisher.set(atPosition()); + appliedCurrentPublisher.set(getAppliedCurrent().in(Amps)); + } } /** A configuration for a MotorSubsystem */ @@ -234,6 +263,7 @@ public static class MotorSubsystemConfiguration { private PIDController controller; private double acceptableError; private double startingPosition; + private NetworkTableInstance ntInstance; /** * Creates a new configuration for a MotorSubsystems. The default acceptable error is {@value @@ -340,5 +370,10 @@ public MotorSubsystemConfiguration rotationUnit(AngleUnit rotationUnit) { this.rotationUnit = rotationUnit; return this; } + + public MotorSubsystemConfiguration publishTo(NetworkTableInstance ntInstance) { + this.ntInstance = ntInstance; + return this; + } } }