{@code PositionalMotor} supports a dual operation mode: + * + *
When the motor is created, PID control is disabled. The current mode of the motor can be + * determined by calling {@link #isPIDControlEnabled()}. + * + *
To enable PID Control Mode call {@link #setSetpoint(P)}; the motor will move toward the + * setpoint and then maintain position at the setpoint under the control of the PID controller. + * + *
The Direct User Input Mode is activated when the user calls the {@link + * #set(ControlMode, double)}. The PID Mode is interrupted and disengaged, and the provided demand + * will be directly sent to the motor. + * + *
To stop the motor, call {@link #disable()}. + * + * @param
an enum implementing {@link Supplier & Supplier The default error tolerance is {@value #DEFAULT_TOLERANCE}, the PID constants are set to 0,
+ * and the rotational unit is set to {@link Units#Rotations}.
+ *
+ * @param periodicRegistry periodic registry that the motor should use
+ * @param motor the integrated motor controller
+ * @return builder instance
+ */
+ public static Builder builder(PeriodicRegistry periodicRegistry, PIDMotor motor) {
+ return new Builder(periodicRegistry, motor, motor);
+ }
+
+ /**
+ * Creates a new builder for a {@code PositionalMotor}.
+ *
+ * The default error tolerance is {@value #DEFAULT_TOLERANCE}, the PID constants are set to 0,
+ * and the rotational unit is set to {@link Units#Rotations}.
+ *
+ * @param periodicRegistry periodic registry that the motor should use
+ * @param motor the motor to control
+ * @param encoder the encoder providing feedback
+ * @return builder instance
+ */
+ public static Builder builder(PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder) {
+ return new Builder(periodicRegistry, motor, encoder);
+ }
+
+ /** A builder for a {@code PositionalMotor}. */
+ public static class Builder {
+
+ private final PeriodicRegistry periodicRegistry;
+ private final Motor motor;
+ private final Encoder encoder;
+ private final PIDController controller;
+ private ControlMode controlMode = ControlMode.DUTY_CYCLE;
+ private AngleUnit rotationUnit = Units.Rotations;
+ private Clamper clamper = value -> value;
+ private NetworkTable networkTable;
+
+ private Builder(PeriodicRegistry periodicRegistry, Motor motor, Encoder encoder) {
+ this.periodicRegistry =
+ Objects.requireNonNull(periodicRegistry, "periodicRegistry should not be null");
+ 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);
+ controller.setTolerance(DEFAULT_TOLERANCE);
+ }
+
+ /**
+ * 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 Builder 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 coefficient
+ * @param i the integral coefficient
+ * @param d the derivative coefficient
+ * @return {@code this} for chaining
+ */
+ public Builder PID(double p, double i, double d) {
+ controller.setPID(p, i, d);
+ return this;
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with {@link #atSetpoint()}.
+ *
+ * @param errorTolerance Error which is tolerable
+ * @return {@code this} for chaining
+ */
+ public Builder tolerance(double errorTolerance) {
+ controller.setTolerance(errorTolerance);
+ return this;
+ }
+
+ /**
+ * Sets the error values which are considered tolerable for use with {@link #atSetpoint()}.
+ *
+ * @param errorTolerance Error which is tolerable
+ * @param errorDerivativeTolerance Error derivative which is tolerable
+ * @return {@code this} for chaining
+ */
+ public Builder tolerance(double errorTolerance, double errorDerivativeTolerance) {
+ controller.setTolerance(errorTolerance, errorDerivativeTolerance);
+ return this;
+ }
+
+ /**
+ * Sets the low and high values for the output to use when PID control is enabled.
+ *
+ * @param low The lower boundary to which to clamp output values.
+ * @param high The higher boundary to which to clamp output values.
+ * @return {@code this} for chaining
+ */
+ public Builder limits(double low, double high) {
+ if (low >= high) {
+ throw new IllegalArgumentException("Low value must be smaller than high value");
+ }
+ clamper = output -> MathUtil.clamp(output, low, high);
+ return this;
+ }
+
+ /**
+ * Sets the function to use to clamp output values when PID control is enabled.
+ *
+ * @param clamper Function to use to clamp output values
+ * @return {@code this} for chaining
+ */
+ public Builder clamper(Clamper clamper) {
+ this.clamper = Objects.requireNonNull(clamper, "clamper should not be null");
+ return this;
+ }
+
+ /**
+ * Sets the unit to use for PID calculations
+ *
+ * @param rotationUnit The angle unit to use for calculations
+ */
+ public Builder rotationUnit(AngleUnit rotationUnit) {
+ this.rotationUnit = rotationUnit;
+ return this;
+ }
+
+ /**
+ * Publish metrics about the motor and controller to the provided table.
+ *
+ * @param networkTable Table to publish to
+ * @return {@code this} for chaining
+ */
+ public Builder publishTo(NetworkTable networkTable) {
+ this.networkTable = Objects.requireNonNull(networkTable, "networkTable should not be null");
+ return this;
+ }
+
+ /**
+ * Builds a {@code PositionalMotor} using the current values of this builder.
+ *
+ * @param initialPosition the initial position of the controller
+ */
+ public & Supplier build(P initialPosition) {
+ Objects.requireNonNull(initialPosition, "initialPosition should not be null");
+ return new PositionalMotor<>(this, initialPosition);
+ }
+ }
+
+ private PositionalMotor(Builder builder, P initialPosition) {
+ controller = builder.controller;
+ motor = builder.motor;
+ encoder = builder.encoder;
+ controlMode = builder.controlMode;
+ rotationUnit = builder.rotationUnit;
+ clamper = builder.clamper;
+
+ if (builder.networkTable != null) {
+ publishers = new Publishers(builder.networkTable);
+ } else {
+ publishers = null;
+ }
+
+ double initialPositionAsDouble = initialPosition.get().in(rotationUnit);
+ controller.setSetpoint(initialPositionAsDouble);
+ encoder.setPosition(rotationUnit.of(initialPositionAsDouble));
+
+ builder.periodicRegistry.addPeriodic(this::periodic);
+ }
+
+ @Override
+ public void initSendable(SendableBuilder builder) {
+ controller.initSendable(builder);
+ }
+
+ /**
+ * Sets the desired setpoint to the provided value, and enables the PID control.
+ *
+ * @param position the position to go to.
+ */
+ public void setSetpoint(P position) {
+ Objects.requireNonNull(position, "position should not be null");
+ if (!isPIDControlEnabled) {
+ enablePIDControl();
+ }
+ double setpoint = position.get().in(rotationUnit);
+ controller.setSetpoint(setpoint);
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Additionally, this method disables PID control of the subsystem
+ */
+ @Override
+ public void set(ControlMode mode, double demand) {
+ isPIDControlEnabled = false;
+ motor.set(mode, demand);
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Additionally, this method disables PID control of the subsystem
+ */
+ @Override
+ public void set(ControlMode mode, double demand, double feedForward) {
+ isPIDControlEnabled = false;
+ motor.set(mode, demand, feedForward);
+ }
+
+ @Override
+ public Current getAppliedCurrent() {
+ return motor.getAppliedCurrent();
+ }
+
+ /**
+ * Engages the PID Controller.
+ *
+ * The motor voltage will be periodically updated to move the motor towards the current
+ * setpoint.
+ *
+ * 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 void enablePIDControl() {
+ isPIDControlEnabled = true;
+ }
+
+ /**
+ * Returns whether PID control is enabled.
+ *
+ * @return Whether PID control is enabled.
+ */
+ public boolean isPIDControlEnabled() {
+ return isPIDControlEnabled;
+ }
+
+ /**
+ * 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() {
+ isPIDControlEnabled = false;
+ motor.stopMotor();
+ }
+
+ /** Returns the current setpoint as an angle. */
+ public Angle getSetpoint() {
+ return rotationUnit.of(controller.getSetpoint());
+ }
+
+ /**
+ * Gets the position of the encoder.
+ *
+ * @return the position of the encoder as a measure
+ */
+ public Angle getPositionMeasure() {
+ return encoder.getPositionMeasure();
+ }
+
+ /** Determines if the motor is at the current setpoint, within the acceptable error. */
+ public boolean atSetpoint() {
+ return controller.atSetpoint();
+ }
+
+ /** Applies the PID output to the motor if this motor is enabled. */
+ private void periodic(RobotState robotState) {
+ if (isPIDControlEnabled) {
+ double nextOutput = controller.calculate(getMeasurement());
+ motor.set(controlMode, clamper.clampValue(nextOutput));
+ }
+ if (publishers != null) {
+ publishers.positionPublisher.set(getPositionMeasure().in(rotationUnit));
+ publishers.setpointPublisher.set(getSetpoint().in(rotationUnit));
+ publishers.atSetpointPublisher.set(atSetpoint());
+ publishers.appliedCurrentPublisher.set(motor.getAppliedCurrent().in(Units.Amps));
+ publishers.pidControlEnabledPublisher.set(isPIDControlEnabled);
+ }
+ }
+
+ @Override
+ public void close() {
+ if (publishers != null) {
+ publishers.close();
+ }
+ }
+
+ private double getMeasurement() {
+ return encoder.getPositionMeasure().in(rotationUnit);
+ }
+
+ private record Publishers(
+ DoublePublisher positionPublisher,
+ BooleanPublisher atSetpointPublisher,
+ DoublePublisher appliedCurrentPublisher,
+ DoublePublisher setpointPublisher,
+ BooleanPublisher pidControlEnabledPublisher) {
+
+ Publishers(NetworkTable networkTable) {
+ this(
+ networkTable.getDoubleTopic("position").publish(),
+ networkTable.getBooleanTopic("at setpoint").publish(),
+ networkTable.getDoubleTopic("applied current").publish(),
+ networkTable.getDoubleTopic("setpoint").publish(),
+ networkTable.getBooleanTopic("PID control enabled").publish());
+ }
+
+ void close() {
+ positionPublisher.close();
+ atSetpointPublisher.close();
+ appliedCurrentPublisher.close();
+ setpointPublisher.close();
+ }
+ }
+}