diff --git a/src/main/java/com/team766/hal/PIDSlotHelper.java b/src/main/java/com/team766/hal/PIDSlotHelper.java index ec3d7b87f..adb953ee8 100644 --- a/src/main/java/com/team766/hal/PIDSlotHelper.java +++ b/src/main/java/com/team766/hal/PIDSlotHelper.java @@ -18,16 +18,16 @@ private Slot(int slot) { this.slot = slot; this.pGain = new ObserveValue( - ObserveValue.whenPresent((p) -> motor.setP(p, this.slot))); + ObserveValue.whenPresent((p) -> motor.setP_Impl(p, this.slot))); this.iGain = new ObserveValue( - ObserveValue.whenPresent((i) -> motor.setI(i, this.slot))); + ObserveValue.whenPresent((i) -> motor.setI_Impl(i, this.slot))); this.dGain = new ObserveValue( - ObserveValue.whenPresent((d) -> motor.setD(d, this.slot))); + ObserveValue.whenPresent((d) -> motor.setD_Impl(d, this.slot))); this.ffGain = new ObserveValue( - ObserveValue.whenPresent((ff) -> motor.setFF(ff, this.slot))); + ObserveValue.whenPresent((ff) -> motor.setFF_Impl(ff, this.slot))); this.outputMin = new ObserveValue(ObserveValue.whenPresent((__) -> updateOutputRange())); this.outputMax = @@ -35,17 +35,39 @@ private Slot(int slot) { } private void updateOutputRange() { - motor.setOutputRange( + motor.setOutputRange_Impl( outputMin.getValueProvider().valueOr(-1.0), outputMax.getValueProvider().valueOr(1.0), slot); } } - private final MotorController motor; + /** + * The methods of this interface match the semantics of the similarly-named methods in + * {@link MotorController}, but it's important these set_Impl methods remain separate from the + * MotorController methods, otherwise PIDSlotHelper will not work properly. + * All of the MotorController methods should call PIDSlotHelper (rather than updating the motor + * device directly) otherwise PIDSlotHelper's ObserveValues will not be updated, and they will + * stay subscribed to the old ValueProvider. + */ + public static interface MotorCallbacks { + int numPIDSlots(); + + void setP_Impl(double value, int slot); + + void setI_Impl(double value, int slot); + + void setD_Impl(double value, int slot); + + void setFF_Impl(double value, int slot); + + void setOutputRange_Impl(double minOutput, double maxOutput, int slot); + } + + private final MotorCallbacks motor; private final Slot[] slots; - public PIDSlotHelper(MotorController motor) { + public PIDSlotHelper(MotorCallbacks motor) { this.motor = motor; final int size = motor.numPIDSlots(); this.slots = new Slot[size]; diff --git a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java index 4a99e8089..0c2078c57 100644 --- a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java @@ -14,7 +14,8 @@ import java.util.function.Function; import java.util.function.Supplier; -public class CANSparkMaxMotorController extends CANSparkMax implements MotorController { +public class CANSparkMaxMotorController extends CANSparkMax + implements MotorController, PIDSlotHelper.MotorCallbacks { private static final int NUM_PID_SLOTS = 2; // should be 4, only exposing 2 at this time. @@ -155,7 +156,7 @@ public void setP(ValueProvider value, int slot) { } @Override - public void setP(final double value, int slot) { + public void setP_Impl(final double value, int slot) { revErrorToException(ExceptionTarget.LOG, getPIDController().setP(value, slot)); } @@ -165,7 +166,7 @@ public void setI(ValueProvider value, int slot) { } @Override - public void setI(final double value, int slot) { + public void setI_Impl(final double value, int slot) { revErrorToException(ExceptionTarget.LOG, getPIDController().setI(value, slot)); } @@ -175,7 +176,7 @@ public void setD(ValueProvider value, int slot) { } @Override - public void setD(final double value, int slot) { + public void setD_Impl(final double value, int slot) { revErrorToException(ExceptionTarget.LOG, getPIDController().setD(value, slot)); } @@ -185,7 +186,7 @@ public void setFF(ValueProvider value, int slot) { } @Override - public void setFF(final double value, int slot) { + public void setFF_Impl(final double value, int slot) { revErrorToException(ExceptionTarget.LOG, getPIDController().setFF(value, slot)); } @@ -282,7 +283,7 @@ public void setOutputRange( } @Override - public void setOutputRange(final double minOutput, final double maxOutput, int slot) { + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { revErrorToException( ExceptionTarget.LOG, getPIDController().setOutputRange(minOutput, maxOutput, slot)); } diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java index 06d2d8d10..5ac780cd6 100644 --- a/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java @@ -27,7 +27,8 @@ import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; -public class CANTalonFxMotorController extends TalonFX implements MotorController { +public class CANTalonFxMotorController extends TalonFX + implements MotorController, PIDSlotHelper.MotorCallbacks { private static final int NUM_PID_SLOTS = 2; @@ -194,7 +195,7 @@ public void setFF(ValueProvider value, int slot) { } @Override - public void setFF(final double value, int slot) { + public void setFF_Impl(final double value, int slot) { refreshConfig(); switch (slot) { case 0: @@ -215,7 +216,7 @@ public void setP(final ValueProvider value, int slot) { } @Override - public void setP(final double value, int slot) { + public void setP_Impl(final double value, int slot) { refreshConfig(); switch (slot) { case 0: @@ -236,7 +237,7 @@ public void setI(final ValueProvider value, int slot) { } @Override - public void setI(final double value, int slot) { + public void setI_Impl(final double value, int slot) { refreshConfig(); switch (slot) { case 0: @@ -257,7 +258,7 @@ public void setD(final ValueProvider value, int slot) { } @Override - public void setD(final double value, int slot) { + public void setD_Impl(final double value, int slot) { refreshConfig(); switch (slot) { case 0: @@ -313,7 +314,7 @@ public void setOutputRange( } @Override - public void setOutputRange(final double minOutput, final double maxOutput, int slot) { + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { if (slot != 0) { Logger.get(Category.HAL) .logRaw( diff --git a/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java b/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java index 8c916680a..3072462c3 100644 --- a/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java @@ -13,7 +13,8 @@ import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; -public class CANTalonMotorController extends BaseCTREMotorController implements MotorController { +public class CANTalonMotorController extends BaseCTREMotorController + implements MotorController, PIDSlotHelper.MotorCallbacks { private static final int NUM_PID_SLOTS = 2; private WPI_TalonSRX m_device; @@ -139,22 +140,22 @@ public void setD(ValueProvider value, int slot) { } @Override - public void setFF(final double value, int slot) { + public void setFF_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(slot, value, TIMEOUT_MS)); } @Override - public void setP(final double value, int slot) { + public void setP_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(slot, value)); } @Override - public void setI(final double value, int slot) { + public void setI_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(slot, value)); } @Override - public void setD(final double value, int slot) { + public void setD_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(slot, value)); } @@ -176,7 +177,7 @@ public void setOutputRange( } @Override - public void setOutputRange(final double minOutput, final double maxOutput, int slot) { + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { if (slot != 0) { Logger.get(Category.HAL) .logRaw( diff --git a/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java b/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java index 8292afb36..6b35b6643 100644 --- a/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java @@ -13,7 +13,8 @@ import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; -public class CANVictorMotorController extends BaseCTREMotorController implements MotorController { +public class CANVictorMotorController extends BaseCTREMotorController + implements MotorController, PIDSlotHelper.MotorCallbacks { private static final int NUM_PID_SLOTS = 2; private final WPI_VictorSPX m_device; @@ -121,7 +122,7 @@ public void setFF(final ValueProvider value, int slot) { } @Override - public void setFF(final double value, int slot) { + public void setFF_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kF(slot, value, TIMEOUT_MS)); } @@ -161,7 +162,7 @@ public void setP(final ValueProvider value, int slot) { } @Override - public void setP(final double value, int slot) { + public void setP_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kP(slot, value, TIMEOUT_MS)); } @@ -171,7 +172,7 @@ public void setI(final ValueProvider value, int slot) { } @Override - public void setI(final double value, int slot) { + public void setI_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kI(slot, value, TIMEOUT_MS)); } @@ -181,7 +182,7 @@ public void setD(final ValueProvider value, int slot) { } @Override - public void setD(final double value, int slot) { + public void setD_Impl(final double value, int slot) { errorCodeToException(ExceptionTarget.LOG, m_device.config_kD(slot, value, TIMEOUT_MS)); } @@ -203,7 +204,7 @@ public void setOutputRange( } @Override - public void setOutputRange(final double minOutput, final double maxOutput, int slot) { + public void setOutputRange_Impl(final double minOutput, final double maxOutput, int slot) { if (slot != 0) { Logger.get(Category.HAL) .logRaw(