From 724bb0ee16944d0c79b7dca1fef9ef83a78091e9 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 11 Jan 2026 20:35:49 -0800 Subject: [PATCH 1/5] Add MotorSubsystem.stopMotorCommand() --- .../com/team2813/lib2813/subsystems/MotorSubsystem.java | 9 +++++++++ 1 file changed, 9 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 7beaa000..158004f2 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -178,6 +178,15 @@ public final void stopMotor() { motor.stopMotor(); } + /** + * Returns a command that stops the motor. + * + * @since 2.0.0 + */ + public final Command stopMotorCommand() { + return new InstantCommand(this::stopMotor, this); + } + /** * Returns whether the PID controller is engaged. * From 7d8dd50cfa10abab948f2372869532b32707158d Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 11 Jan 2026 22:53:45 -0800 Subject: [PATCH 2/5] Update MotorSubsystem to not implement Motor or Encoder The goal is to discourage code outside of the subsystem to directly interact with the motor. Subclasses should instead provide public APIs for controlling the subsystem, preferably via commands. --- .../lib2813/subsystems/MotorSubsystem.java | 226 +++++++++++------- 1 file changed, 137 insertions(+), 89 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 158004f2..b3015b81 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -22,6 +22,7 @@ import com.team2813.lib2813.control.Encoder; import com.team2813.lib2813.control.Motor; import com.team2813.lib2813.control.PIDMotor; +import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.DoublePublisher; @@ -32,6 +33,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.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -55,37 +58,33 @@ * controller. The motor system's {@link #isEnabled()} returns {@code true}. * *

The Direct User Input Mode is activated when the user calls the {@link - * #set(ControlMode,double,double)} or {@link #set(ControlMode,double)} method, where the user - * provides direct input of type ControlType (specified via {@link - * MotorSubsystemConfiguration#controlMode(ControlMode)}). The PID Mode is interrupted and - * disengaged, and {@link #isEnabled()} returns {@code false}. It can be re-engaged with the {@link - * #enable()} method and will resume movement toward setpoint. + * Motor#set(ControlMode,double,double)} or {@link Motor#set(ControlMode,double)} method on the + * {@link #motor} field. The PID Mode is interrupted and disengaged, and {@link #isEnabled()} + * returns {@code false}. It can be re-engaged with the {@link #enable()} method and will resume + * movement toward setpoint. * * @param the type of the {@link Supplier} used to specify setpoints. */ -public abstract class MotorSubsystem> extends SubsystemBase - implements Motor, Encoder { +public abstract class MotorSubsystem> extends SubsystemBase { - protected final Motor motor; + protected final ControlledMotor motor; protected final Encoder encoder; protected final ControlMode controlMode; protected final AngleUnit rotationUnit; - protected final double acceptableError; - protected final PIDController controller; + private final PIDController controller; private final DoublePublisher positionPublisher; private final BooleanPublisher atPositionPublisher; private final DoublePublisher appliedCurrentPublisher; private final DoublePublisher setpointPublisher; - private boolean isEnabled; + private boolean pidControlEnabled; protected MotorSubsystem(MotorSubsystemConfiguration builder) { controller = builder.controller; controller.setTolerance(builder.acceptableError); controller.setSetpoint(builder.startingPosition); - acceptableError = builder.acceptableError; - motor = builder.motor; - encoder = builder.encoder; + motor = new ControlledMotor(builder.motor); + encoder = new EncoderWrapper(builder.encoder); controlMode = builder.controlMode; rotationUnit = builder.rotationUnit; if (builder.ntInstance != null) { @@ -100,6 +99,7 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) { appliedCurrentPublisher = null; setpointPublisher = null; } + addChild("controller", new PIDSendable()); } /** @@ -108,9 +108,7 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) { * @param position the position to go to. */ public final void setSetpoint(T position) { - if (!isEnabled()) { - enable(); - } + pidControlEnabled = true; double setpoint = position.get().in(rotationUnit); controller.setSetpoint(setpoint); } @@ -132,23 +130,7 @@ public final Angle getSetpoint() { /** Determines if the motor is at the current setpoint, within the acceptable error. */ public final boolean atPosition() { - return Math.abs(getMeasurement() - controller.getSetpoint()) <= acceptableError; - } - - /** - * {@inheritDoc} - * - *

Additionally, this method disables PID control of the subsystem - */ - @Override - public final void set(ControlMode mode, double demand, double feedForward) { - isEnabled = false; - motor.set(mode, demand, feedForward); - } - - @Override - public final Current getAppliedCurrent() { - return motor.getAppliedCurrent(); + return controller.atSetpoint(); } /** @@ -161,30 +143,7 @@ public final Current getAppliedCurrent() { * interruption, the motor will resume its movement towards the last set setpoint. */ public final void enable() { - isEnabled = true; - } - - /** - * Stops the motor. - * - *

The motor voltage will be set to zero, and the motor will not adjust to move towards the - * current setpoint. - * - * @since 2.0.0 - */ - @Override - public final void stopMotor() { - isEnabled = false; - motor.stopMotor(); - } - - /** - * Returns a command that stops the motor. - * - * @since 2.0.0 - */ - public final Command stopMotorCommand() { - return new InstantCommand(this::stopMotor, this); + pidControlEnabled = true; } /** @@ -197,28 +156,28 @@ public final Command stopMotorCommand() { * @return Whether the PID controller is engaged. */ public final boolean isEnabled() { - return isEnabled; + return pidControlEnabled; } /** - * {@inheritDoc} + * Stops the motor. + * + *

The motor voltage will be set to zero, and the motor will not adjust to move towards the + * current setpoint. * - *

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

This is called by {@link #periodic()} if this subsystem is enabled. + * @since 2.0.0 */ - private void useOutput(double output) { - motor.set(controlMode, clampOutput(output)); + public final Command stopMotorCommand() { + return new InstantCommand(this::stopMotor, this); } /** @@ -244,32 +203,18 @@ protected final double getMeasurement() { return encoder.getPositionMeasure().in(rotationUnit); } - @Override - public final Angle getPositionMeasure() { - return encoder.getPositionMeasure(); - } - - @Override - public final void setPosition(Angle position) { - encoder.setPosition(position); - } - - @Override - 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) { - useOutput(controller.calculate(getMeasurement())); + if (pidControlEnabled) { + double position = getMeasurement(); + motor.set(controlMode, clampOutput(position)); } if (positionPublisher != null) { - positionPublisher.set(getPositionMeasure().in(Rotations)); + positionPublisher.set(encoder.getPositionMeasure().in(Rotations)); setpointPublisher.set(getSetpoint().in(Rotations)); atPositionPublisher.set(atPosition()); - appliedCurrentPublisher.set(getAppliedCurrent().in(Amps)); + appliedCurrentPublisher.set(motor.getAppliedCurrent().in(Amps)); } } @@ -422,4 +367,107 @@ public MotorSubsystemConfiguration publishTo(NetworkTableInstance ntInstance) { return this; } } + + /** A motor that is controlled by a subclass of {@link MotorSubsystem}. */ + protected final class ControlledMotor implements Motor { + private final Motor motor; + + private ControlledMotor(Motor motor) { + this.motor = motor; + } + + /** + * {@inheritDoc} + * + *

Additionally, this method disables PID control of the subsystem + */ + @Override + public void set(ControlMode mode, double demand) { + pidControlEnabled = false; + motor.set(mode, demand); + } + + /** + * {@inheritDoc} + * + *

Additionally, this method disables PID control of the subsystem. It does not + * clamp the provided value. + */ + @Override + public void set(ControlMode mode, double demand, double feedForward) { + pidControlEnabled = false; + motor.set(mode, demand, feedForward); + } + + @Override + public Current getAppliedCurrent() { + return motor.getAppliedCurrent(); + } + + /** + * Stops the motor. + * + *

The motor voltage will be set to zero, and the motor will not adjust to move towards the + * current setpoint. + */ + @Override + public void stopMotor() { + pidControlEnabled = false; + motor.stopMotor(); + } + } + + private static class EncoderWrapper implements Encoder { + private final Encoder encoder; + + EncoderWrapper(Encoder encoder) { + this.encoder = encoder; + } + + @Override + public Angle getPositionMeasure() { + return encoder.getPositionMeasure(); + } + + @Override + public void setPosition(Angle position) { + encoder.setPosition(position); + } + + @Override + public AngularVelocity getVelocityMeasure() { + return encoder.getVelocityMeasure(); + } + } + + /** A Sendable implementation that allows users to update PID settings. */ + private class PIDSendable implements Sendable { + + /** + * {@inheritDoc + * + *

This is modeled after {@link PIDController#initSendable(SendableBuilder)}, + * but does not support changing the setpoint. + */ + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PIDController"); + builder.addDoubleProperty("p", controller::getP, controller::setP); + builder.addDoubleProperty("i", controller::getI, controller::setI); + builder.addDoubleProperty("d", controller::getD, controller::setD); + builder.addDoubleProperty( + "izone", + controller::getIZone, + (double toSet) -> { + try { + controller.setIZone(toSet); + } catch (IllegalArgumentException e) { + MathSharedStore.reportError( + "IZone must be a non-negative number!", e.getStackTrace()); + } + }); + builder.addDoubleProperty( + "acceptableError", controller::getErrorTolerance, controller::setTolerance); + } + } } From 6b1fc7ff97a62cd4ae90a0974656ab6d24a179bc Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 1 Feb 2026 13:18:08 -0800 Subject: [PATCH 3/5] Remove EncoderWrapper --- .../lib2813/subsystems/MotorSubsystem.java | 26 +------------------ 1 file changed, 1 insertion(+), 25 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 b3015b81..1ca07925 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -31,7 +31,6 @@ import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.Units; 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.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; @@ -84,7 +83,7 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) { controller.setTolerance(builder.acceptableError); controller.setSetpoint(builder.startingPosition); motor = new ControlledMotor(builder.motor); - encoder = new EncoderWrapper(builder.encoder); + encoder = builder.encoder; controlMode = builder.controlMode; rotationUnit = builder.rotationUnit; if (builder.ntInstance != null) { @@ -417,29 +416,6 @@ public void stopMotor() { } } - private static class EncoderWrapper implements Encoder { - private final Encoder encoder; - - EncoderWrapper(Encoder encoder) { - this.encoder = encoder; - } - - @Override - public Angle getPositionMeasure() { - return encoder.getPositionMeasure(); - } - - @Override - public void setPosition(Angle position) { - encoder.setPosition(position); - } - - @Override - public AngularVelocity getVelocityMeasure() { - return encoder.getVelocityMeasure(); - } - } - /** A Sendable implementation that allows users to update PID settings. */ private class PIDSendable implements Sendable { From d6d2962fee330e3d6feb062e45495a8ca1129220 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 1 Feb 2026 13:24:26 -0800 Subject: [PATCH 4/5] Prevent negative values from being passed to PIDController.setTolerance() --- .../com/team2813/lib2813/subsystems/MotorSubsystem.java | 8 +++++++- 1 file changed, 7 insertions(+), 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 1ca07925..bfa9ee57 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -336,8 +336,12 @@ public MotorSubsystemConfiguration startingPosition(Supplier startingPosi * * @param error the error which is considered tolerable for use with {@code }atPosition()} * @return {@code this} for chaining + * @throws IllegalArgumentException If {@code error} is negative */ public MotorSubsystemConfiguration acceptableError(double error) { + if (error < 0) { + throw new IllegalArgumentException("AcceptableError cannot be negative"); + } this.acceptableError = error; return this; } @@ -443,7 +447,9 @@ public void initSendable(SendableBuilder builder) { } }); builder.addDoubleProperty( - "acceptableError", controller::getErrorTolerance, controller::setTolerance); + "acceptableError", + controller::getErrorTolerance, + t -> controller.setTolerance(Math.abs(t))); } } } From 1eacefac41ad107f9f9d9a98abf6f824ba3bfa2b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 1 Feb 2026 13:27:00 -0800 Subject: [PATCH 5/5] Change version from 2.0.0-rc-1 to 2.0.0-rc-2 --- buildSrc/src/main/groovy/publishing-conventions.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buildSrc/src/main/groovy/publishing-conventions.gradle b/buildSrc/src/main/groovy/publishing-conventions.gradle index a26d596b..b22f05d2 100644 --- a/buildSrc/src/main/groovy/publishing-conventions.gradle +++ b/buildSrc/src/main/groovy/publishing-conventions.gradle @@ -2,7 +2,7 @@ plugins { id 'maven-publish' } -version = '2.0.0-rc-1' +version = '2.0.0-rc-2' publishing { publications {