The MotorSybsystem supports a dual operation mode: + * + *
The PID Mode is enabled by calling {@link setSetpoint(T)}. The subsystem starts moving + * toward the setpoint and maintains position at the setpoint under the control of the PID + * controller. The motor system's {@link isEnabled()} returns {@code true}. + * + *
The Direct User Input Mode is activated when the user calls the {@link
+ * set(ControlType,double,double)} or {@link set(ControlType,double)} method, where the user
+ * provides direct input of type ControlType (specified via {@link
+ * MotorSubsystemConfiguration#controlMode(ControlType)}). 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 This is provided to allow the subclass to define an {@code Enum} (that implements {@code
+ * Supplier This is the acceptable delta between the user-specified setpoint and the actual subsystem
+ * position where the system is reporting to be "in position" (see {@link atPosition}).
+ */
+ public MotorSubsystemConfiguration acceptableError(double error) {
+ this.acceptableError = error;
+ return this;
+ }
+
+ /**
+ * Sets the unit to use for PID calculations.
+ *
+ * @param rotationUnit The angle unit to use for calculations
+ */
+ public MotorSubsystemConfiguration rotationUnit(AngleUnit rotationUnit) {
+ startingPosition = rotationUnit.convertFrom(startingPosition, this.rotationUnit);
+ this.rotationUnit = rotationUnit;
+ return this;
+ }
+
+ /** Sets the network table instance to report subsystem internal state to. */
+ public MotorSubsystemConfiguration publishTo(NetworkTableInstance ntInstance) {
+ this.ntInstance = ntInstance;
+ return this;
+ }
+ }
protected MotorSubsystem(MotorSubsystemConfiguration builder) {
- this.controller = builder.controller;
- this.controller.setTolerance(builder.acceptableError);
+ controller = builder.controller;
+ controller.setTolerance(builder.acceptableError);
acceptableError = builder.acceptableError;
motor = builder.motor;
encoder = builder.encoder;
@@ -65,7 +229,7 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) {
setpointPublisher = null;
}
- this.controller.setSetpoint(builder.startingPosition);
+ controller.setSetpoint(builder.startingPosition);
}
/**
@@ -101,28 +265,13 @@ public final boolean atPosition() {
}
/**
- * {@inheritDoc}
- *
- * Additionally, this method disables PID control of the subsystem
- */
- @Override
- public final void set(ControlMode mode, double demand, double feedForward) {
- if (isEnabled()) {
- disable();
- }
- motor.set(mode, demand, feedForward);
- }
-
- @Override
- public final Current getAppliedCurrent() {
- return motor.getAppliedCurrent();
- }
-
- /**
- * Enables the motor
+ * Engages the PID Controller.
*
* The motor voltage will be periodically updated to move the motor towards the current
* setupoint.
+ *
+ * If this method is called after the motor drive toward setpoint was interrupted by user
+ * interruption, the motor will resume its movement towards the last set setpoint.
*/
public final void enable() {
isEnabled = true;
@@ -140,26 +289,18 @@ public final void disable() {
}
/**
- * Returns whether the controller is enabled. If this is enabled, then PID control will be used.
+ * Returns whether the PID controller is engaged.
*
- * @return Whether the controller is enabled.
+ * When the PID controller is engaged, the motor system is moving toward the user-specified
+ * setpoint position (set via {@link setSetpoint(T)}), or, if it has already reached the setpoint,
+ * is maintaining that position.
+ *
+ * @return Whether the PID controller is engaged.
*/
public final boolean isEnabled() {
return isEnabled;
}
- /**
- * {@inheritDoc}
- *
- * Additionally, this method disables PID control of the subsystem. It does not clamp
- * the provided value.
- */
- @Override
- public final void set(ControlMode mode, double demand) {
- isEnabled = false;
- motor.set(mode, demand);
- }
-
/**
* Clamps the given output value and provides it to the motor.
*
@@ -197,47 +338,96 @@ protected double clampOutput(double output) {
return output;
}
+ // Returns the current position of the motor subsystem in the units specified by m_rotationUnit.
+ // This position is used by the PID controller (when it is enabled) to determine if it is already
+ // at position or needs to adjust the subsystem position to reach a user-specified setpoint.
protected final double getMeasurement() {
return encoder.getPositionMeasure().in(rotationUnit);
}
+ // `Motor` method overrides.
+ // -------------------------
+
+ /**
+ * {@inheritDoc}
+ *
+ * Additionally, this method disengages PID control of the subsystem.
+ */
+ @Override
+ public final void set(ControlMode mode, double demand, double feedForward) {
+ if (isEnabled()) {
+ disable();
+ }
+ motor.set(mode, demand, feedForward);
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Additionally, this method disables PID control of the subsystem. It does not clamp
+ * the provided value.
+ */
+ @Override
+ public final void set(ControlMode mode, double demand) {
+ isEnabled = false;
+ motor.set(mode, demand);
+ }
+
+ /** {@inheritDoc} */
+ @Override
+ public final Current getAppliedCurrent() {
+ return motor.getAppliedCurrent();
+ }
+
+ // `Encoder` method overrides.
+ // ---------------------------
+
+ /** {@inheritDoc} */
@Override
@Deprecated(forRemoval = true)
public double position() {
return encoder.position();
}
+ /** {@inheritDoc} */
@Override
public final Angle getPositionMeasure() {
return encoder.getPositionMeasure();
}
+ /** {@inheritDoc} */
@Override
@Deprecated(forRemoval = true)
public void setPosition(double position) {
encoder.setPosition(position);
}
+ /** {@inheritDoc} */
@Override
public final void setPosition(Angle position) {
encoder.setPosition(position);
}
+ /** {@inheritDoc} */
@Override
@Deprecated(forRemoval = true)
public double getVelocity() {
return encoder.getVelocity();
}
+ /** {@inheritDoc} */
@Override
public final AngularVelocity getVelocityMeasure() {
return encoder.getVelocityMeasure();
}
+ // `Subsystem` method overrides.
+ // -----------------------------
+
/** Applies the PID output to the motor if this subsystem is enabled. */
@Override
public void periodic() {
- if (isEnabled) {
+ if (isEnabled()) {
useOutput(controller.calculate(getMeasurement()));
}
if (positionPublisher != null) {
@@ -247,141 +437,4 @@ public void periodic() {
appliedCurrentPublisher.set(getAppliedCurrent().in(Amps));
}
}
-
- /** A configuration for a MotorSubsystem */
- public static class MotorSubsystemConfiguration {
- /** The default acceptable position error. */
- public static final double DEFAULT_ERROR = 5.0;
-
- /** The default starting position if one is not provided. */
- public static final double DEFAULT_STARTING_POSITION = 0.0;
-
- private ControlMode controlMode;
- private AngleUnit rotationUnit;
- private final Motor motor;
- private final Encoder encoder;
- 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
- * #DEFAULT_ERROR}, the PID constants are set to 0, and the starting position is {@value
- * #DEFAULT_STARTING_POSITION}
- *
- * @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");
- this.encoder = Objects.requireNonNull(encoder, "encoder should not be null");
- controller = new PIDController(0, 0, 0);
- acceptableError = DEFAULT_ERROR;
- startingPosition = DEFAULT_STARTING_POSITION;
- controlMode = ControlMode.DUTY_CYCLE;
- rotationUnit = Units.Rotations;
- }
-
- /**
- * Creates a new config for MotorSubsystems using a motor that has a built-in encoder. 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 integrated motor controller
- */
- public MotorSubsystemConfiguration(PIDMotor motor) {
- this(motor, motor);
- }
-
- /**
- * Sets the controller used to calculate the next value
- *
- * @param controller The PID controller
- * @return {@code this} for chaining
- */
- public MotorSubsystemConfiguration controller(PIDController controller) {
- this.controller = controller;
- return this;
- }
-
- /**
- * Sets the control mode to use when giving output to the motor. Defaults to {@link
- * ControlMode#DUTY_CYCLE}.
- *
- * @param controlMode The mode to use when controlling the motor
- * @return {@code this} for chaining
- * @throws IllegalArgumentException If {@code controlMode} is for positional control
- */
- public MotorSubsystemConfiguration controlMode(ControlMode controlMode) {
- if (controlMode.isPositionalControl()) {
- throw new IllegalArgumentException(
- String.format(
- "Control mode %s is for positional control. This is invalid! Please use a different"
- + " control mode",
- controlMode));
- }
- this.controlMode = controlMode;
- return this;
- }
-
- /**
- * Sets the PID constants for the controller
- *
- * @param p the proportional
- * @param i the integral
- * @param d the derivative
- * @return {@code this} for chaining
- */
- public MotorSubsystemConfiguration PID(double p, double i, double d) {
- controller.setPID(p, i, d);
- return this;
- }
-
- /**
- * Sets the initial setpoint of the controller
- *
- * @param startingPosition the initial setpoint
- * @return {@code this} for chaining
- */
- public MotorSubsystemConfiguration startingPosition(Angle startingPosition) {
- this.startingPosition = startingPosition.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