Skip to content

Commit

Permalink
Separate interface for PIDSlotHelper.MotorCallbacks
Browse files Browse the repository at this point in the history
  • Loading branch information
@rcahoon
rcahoon committed Mar 15, 2024
1 parent 28a84c7 commit 3844c09
Show file tree
Hide file tree
Showing 5 changed files with 57 additions and 31 deletions.
36 changes: 29 additions & 7 deletions src/main/java/com/team766/hal/PIDSlotHelper.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,34 +18,56 @@ private Slot(int slot) {
this.slot = slot;
this.pGain =
new ObserveValue<Double>(
ObserveValue.whenPresent((p) -> motor.setP(p, this.slot)));
ObserveValue.whenPresent((p) -> motor.setP_Impl(p, this.slot)));
this.iGain =
new ObserveValue<Double>(
ObserveValue.whenPresent((i) -> motor.setI(i, this.slot)));
ObserveValue.whenPresent((i) -> motor.setI_Impl(i, this.slot)));
this.dGain =
new ObserveValue<Double>(
ObserveValue.whenPresent((d) -> motor.setD(d, this.slot)));
ObserveValue.whenPresent((d) -> motor.setD_Impl(d, this.slot)));
this.ffGain =
new ObserveValue<Double>(
ObserveValue.whenPresent((ff) -> motor.setFF(ff, this.slot)));
ObserveValue.whenPresent((ff) -> motor.setFF_Impl(ff, this.slot)));
this.outputMin =
new ObserveValue<Double>(ObserveValue.whenPresent((__) -> updateOutputRange()));
this.outputMax =
new ObserveValue<Double>(ObserveValue.whenPresent((__) -> updateOutputRange()));
}

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];
Expand Down
13 changes: 7 additions & 6 deletions src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand Down Expand Up @@ -155,7 +156,7 @@ public void setP(ValueProvider<Double> 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));
}

Expand All @@ -165,7 +166,7 @@ public void setI(ValueProvider<Double> 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));
}

Expand All @@ -175,7 +176,7 @@ public void setD(ValueProvider<Double> 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));
}

Expand All @@ -185,7 +186,7 @@ public void setFF(ValueProvider<Double> 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));
}

Expand Down Expand Up @@ -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));
}
Expand Down
13 changes: 7 additions & 6 deletions src/main/java/com/team766/hal/wpilib/CANTalonFxMotorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -194,7 +195,7 @@ public void setFF(ValueProvider<Double> 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:
Expand All @@ -215,7 +216,7 @@ public void setP(final ValueProvider<Double> 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:
Expand All @@ -236,7 +237,7 @@ public void setI(final ValueProvider<Double> 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:
Expand All @@ -257,7 +258,7 @@ public void setD(final ValueProvider<Double> 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:
Expand Down Expand Up @@ -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(
Expand Down
13 changes: 7 additions & 6 deletions src/main/java/com/team766/hal/wpilib/CANTalonMotorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -139,22 +140,22 @@ public void setD(ValueProvider<Double> 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));
}

Expand All @@ -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(
Expand Down
13 changes: 7 additions & 6 deletions src/main/java/com/team766/hal/wpilib/CANVictorMotorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -121,7 +122,7 @@ public void setFF(final ValueProvider<Double> 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));
}

Expand Down Expand Up @@ -161,7 +162,7 @@ public void setP(final ValueProvider<Double> 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));
}

Expand All @@ -171,7 +172,7 @@ public void setI(final ValueProvider<Double> 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));
}

Expand All @@ -181,7 +182,7 @@ public void setD(final ValueProvider<Double> 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));
}

Expand All @@ -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(
Expand Down

0 comments on commit 3844c09

Please sign in to comment.