diff --git a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java index fc3f38e8..886a8c71 100644 --- a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java +++ b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java @@ -34,6 +34,8 @@ import frc.spectrumLib.util.CanDeviceId; import frc.spectrumLib.util.Conversions; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + import lombok.*; /** @@ -48,6 +50,9 @@ public abstract class Mechanism implements NTSendable, SpectrumSubsystem { public Config config; Alert currentAlert = new Alert("", AlertType.kWarning); + Alert disconnectedAlert = + new Alert(getName() + " motor " + config.getId() + " disconnected", AlertType.kWarning); + Alert unlicensedAlert = new Alert(getName() + " motor " + config.getId() + " unlicensed", AlertType.kWarning); private double target = 0; private final CachedDouble cachedRotations; @@ -468,15 +473,35 @@ protected void tareMotor() { } } + protected void checkMotorOK(Supplier motorRequest) { + if (motorRequest.get().isOK()) { + disconnectedAlert.set(false); + unlicensedAlert.set(false); + return; + } + + if (motorRequest.get().isOK()) { + return; + } + + disconnectedAlert.set(true); + + if (motorRequest.get().value == StatusCode.UnlicensedDevice.value) { + unlicensedAlert.set(true); + } + } + /** * Sets the mechanism position of the motor * * @param rotations rotations */ protected void setMotorPosition(DoubleSupplier rotations) { - if (isAttached()) { - motor.setPosition(rotations.getAsDouble()); + if (!isAttached()) { + disconnectedAlert.set(false); + return; } + checkMotorOK(() -> motor.setPosition(rotations.getAsDouble())); } /** @@ -487,8 +512,10 @@ protected void setMotorPosition(DoubleSupplier rotations) { protected void setMMVelocityFOC(DoubleSupplier velocityRPS) { if (isAttached()) { target = velocityRPS.getAsDouble(); - MotionMagicVelocityTorqueCurrentFOC mm = config.mmVelocityFOC.withVelocity(target); - motor.setControl(mm); + MotionMagicVelocityTorqueCurrentFOC mm = + config.mmVelocityFOC.withVelocity(target); + + checkMotorOK(() -> motor.setControl(mm)); } } @@ -501,7 +528,7 @@ protected void setVelocityTorqueCurrentFOC(DoubleSupplier velocityRPS) { if (isAttached()) { target = velocityRPS.getAsDouble(); VelocityTorqueCurrentFOC output = config.velocityTorqueCurrentFOC.withVelocity(target); - motor.setControl(output); + checkMotorOK(() -> motor.setControl(output)); } } @@ -514,7 +541,7 @@ protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPS) { if (isAttached()) { target = Conversions.RPMtoRPS(velocityRPS.getAsDouble()); VelocityTorqueCurrentFOC output = config.velocityTorqueCurrentFOC.withVelocity(target); - motor.setControl(output); + checkMotorOK(() -> motor.setControl(output)); } } @@ -527,7 +554,7 @@ protected void setVelocity(DoubleSupplier velocityRPS) { if (isAttached()) { target = velocityRPS.getAsDouble(); VelocityVoltage output = config.velocityControl.withVelocity(target); - motor.setControl(output); + checkMotorOK(() -> motor.setControl(output)); } } @@ -540,7 +567,7 @@ protected void setMMPositionFoc(DoubleSupplier rotations) { if (isAttached()) { target = rotations.getAsDouble(); MotionMagicTorqueCurrentFOC mm = config.mmPositionFOC.withPosition(target); - motor.setControl(mm); + checkMotorOK(() -> motor.setControl(mm)); } } @@ -566,7 +593,7 @@ protected void setDynMMPositionFoc( .withVelocity(velocity.getAsDouble()) .withAcceleration(acceleration.getAsDouble()) .withJerk(jerk.getAsDouble()); - motor.setControl(mm); + checkMotorOK(() -> motor.setControl(mm)); } } @@ -590,7 +617,7 @@ public void setMMPosition(DoubleSupplier rotations, int slot) { target = rotations.getAsDouble(); MotionMagicVoltage mm = config.mmPositionVoltageSlot.withSlot(slot).withPosition(target); - motor.setControl(mm); + checkMotorOK(() -> motor.setControl(mm)); } } @@ -604,21 +631,21 @@ public void setPercentOutput(DoubleSupplier percent) { VoltageOut output = config.voltageControl.withOutput( config.voltageCompSaturation * percent.getAsDouble()); - motor.setControl(output); + checkMotorOK(() -> motor.setControl(output)); } } public void setVoltageOutput(DoubleSupplier voltage) { if (isAttached()) { VoltageOut output = config.voltageControl.withOutput(voltage.getAsDouble()); - motor.setControl(output); + checkMotorOK(() -> motor.setControl(output)); } } public void setTorqueCurrentFoc(DoubleSupplier current) { if (isAttached()) { TorqueCurrentFOC output = config.torqueCurrentFOC.withOutput(current.getAsDouble()); - motor.setControl(output); + checkMotorOK(() -> motor.setControl(output)); } }