From 82e56b2753b5675cbfaecd33a2794aabd09de2d6 Mon Sep 17 00:00:00 2001 From: cycIes Date: Fri, 13 Dec 2024 12:37:07 -0600 Subject: [PATCH 1/3] added status code checks to motor control methods --- .../frc/spectrumLib/mechanism/Mechanism.java | 152 ++++++++++++++---- 1 file changed, 119 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java index 8231af0a..d0b28b3a 100644 --- a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java +++ b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java @@ -45,6 +45,8 @@ public abstract class Mechanism implements NTSendable, SpectrumSubsystem { public Config config; Alert currentAlert = new Alert("", AlertType.kWarning); + Alert motorDisconnectedAlert = + new Alert("Motor " + config.getId() + " disconnected", AlertType.kWarning); private final CachedDouble cachedRotations; private final CachedDouble cachedPercentage; @@ -350,8 +352,18 @@ protected void tareMotor() { * @param rotations rotations */ protected void setMotorPosition(DoubleSupplier rotations) { - if (isAttached()) { - motor.setPosition(rotations.getAsDouble()); + if (!isAttached()) { + return; + } + + StatusCode response = motor.setPosition(rotations.getAsDouble()); + if (response.isOK()) { + return; + } + + response = motor.setPosition(rotations.getAsDouble()); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -361,10 +373,21 @@ protected void setMotorPosition(DoubleSupplier rotations) { * @param velocityRPS rotations per second */ protected void setMMVelocityFOC(DoubleSupplier velocityRPS) { - if (isAttached()) { - MotionMagicVelocityTorqueCurrentFOC mm = - config.mmVelocityFOC.withVelocity(velocityRPS.getAsDouble()); - motor.setControl(mm); + if (!isAttached()) { + return; + } + + MotionMagicVelocityTorqueCurrentFOC mm = + config.mmVelocityFOC.withVelocity(velocityRPS.getAsDouble()); + + StatusCode response = motor.setControl(mm); + if (response.isOK()) { + return; + } + + response = motor.setControl(mm); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -374,10 +397,21 @@ protected void setMMVelocityFOC(DoubleSupplier velocityRPS) { * @param velocityRPS rotations per second */ protected void setVelocityTorqueCurrentFOC(DoubleSupplier velocityRPS) { - if (isAttached()) { - VelocityTorqueCurrentFOC output = - config.velocityTorqueCurrentFOC.withVelocity(velocityRPS.getAsDouble()); - motor.setControl(output); + if (!isAttached()) { + return; + } + + VelocityTorqueCurrentFOC output = + config.velocityTorqueCurrentFOC.withVelocity(velocityRPS.getAsDouble()); + + StatusCode response = motor.setControl(output); + if (response.isOK()) { + return; + } + + response = motor.setControl(output); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -387,11 +421,22 @@ protected void setVelocityTorqueCurrentFOC(DoubleSupplier velocityRPS) { * @param velocity rotations per second */ protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPS) { - if (isAttached()) { - VelocityTorqueCurrentFOC output = - config.velocityTorqueCurrentFOC.withVelocity( - Conversions.RPMtoRPS(velocityRPS.getAsDouble())); - motor.setControl(output); + if (!isAttached()) { + return; + } + + VelocityTorqueCurrentFOC output = + config.velocityTorqueCurrentFOC.withVelocity( + Conversions.RPMtoRPS(velocityRPS.getAsDouble())); + + StatusCode response = motor.setControl(output); + if (response.isOK()) { + return; + } + + response = motor.setControl(output); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -401,9 +446,20 @@ protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPS) { * @param velocityRPS rotations per second */ protected void setVelocity(DoubleSupplier velocityRPS) { - if (isAttached()) { - VelocityVoltage output = config.velocityControl.withVelocity(velocityRPS.getAsDouble()); - motor.setControl(output); + if (!isAttached()) { + return; + } + + VelocityVoltage output = config.velocityControl.withVelocity(velocityRPS.getAsDouble()); + + StatusCode response = motor.setControl(output); + if (response.isOK()) { + return; + } + + response = motor.setControl(output); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -413,10 +469,20 @@ protected void setVelocity(DoubleSupplier velocityRPS) { * @param rotations rotations */ protected void setMMPositionFoc(DoubleSupplier rotations) { - if (isAttached()) { - MotionMagicTorqueCurrentFOC mm = - config.mmPositionFOC.withPosition(rotations.getAsDouble()); - motor.setControl(mm); + if (!isAttached()) { + return; + } + + MotionMagicTorqueCurrentFOC mm = config.mmPositionFOC.withPosition(rotations.getAsDouble()); + + StatusCode response = motor.setControl(mm); + if (response.isOK()) { + return; + } + + response = motor.setControl(mm); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -436,12 +502,21 @@ protected void setMMPosition(DoubleSupplier rotations) { * @param slot gains slot */ public void setMMPosition(DoubleSupplier rotations, int slot) { - if (isAttached()) { - MotionMagicVoltage mm = - config.mmPositionVoltageSlot - .withSlot(slot) - .withPosition(rotations.getAsDouble()); - motor.setControl(mm); + if (!isAttached()) { + return; + } + + MotionMagicVoltage mm = + config.mmPositionVoltageSlot.withSlot(slot).withPosition(rotations.getAsDouble()); + + StatusCode response = motor.setControl(mm); + if (response.isOK()) { + return; + } + + response = motor.setControl(mm); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } @@ -451,11 +526,22 @@ public void setMMPosition(DoubleSupplier rotations, int slot) { * @param percent fractional units between -1 and +1 */ public void setPercentOutput(DoubleSupplier percent) { - if (isAttached()) { - VoltageOut output = - config.voltageControl.withOutput( - config.voltageCompSaturation * percent.getAsDouble()); - motor.setControl(output); + if (!isAttached()) { + return; + } + + VoltageOut output = + config.voltageControl.withOutput( + config.voltageCompSaturation * percent.getAsDouble()); + + StatusCode response = motor.setControl(output); + if (response.isOK()) { + return; + } + + response = motor.setControl(output); + if (!response.isOK()) { + motorDisconnectedAlert.set(true); } } From afc957abaa2429d823f26a14685f78f17aabcac3 Mon Sep 17 00:00:00 2001 From: cycIes Date: Fri, 13 Dec 2024 15:28:50 -0600 Subject: [PATCH 2/3] moved status code checks into one method --- .../frc/spectrumLib/mechanism/Mechanism.java | 97 +++++-------------- 1 file changed, 23 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java index d0b28b3a..0154adac 100644 --- a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java +++ b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java @@ -31,6 +31,8 @@ import frc.spectrumLib.util.CanDeviceId; import frc.spectrumLib.util.Conversions; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + import lombok.*; /** @@ -46,7 +48,7 @@ public abstract class Mechanism implements NTSendable, SpectrumSubsystem { Alert currentAlert = new Alert("", AlertType.kWarning); Alert motorDisconnectedAlert = - new Alert("Motor " + config.getId() + " disconnected", AlertType.kWarning); + new Alert(getName() + " motor " + config.getId() + " disconnected", AlertType.kWarning); private final CachedDouble cachedRotations; private final CachedDouble cachedPercentage; @@ -346,6 +348,17 @@ protected void tareMotor() { } } + protected void checkMotorOK(Supplier motorRequest) { + if (motorRequest.get().isOK()) { + motorDisconnectedAlert.set(false); + return; + } + + if (!motorRequest.get().isOK()) { + motorDisconnectedAlert.set(true); + } + } + /** * Sets the mechanism position of the motor * @@ -353,18 +366,10 @@ protected void tareMotor() { */ protected void setMotorPosition(DoubleSupplier rotations) { if (!isAttached()) { + motorDisconnectedAlert.set(false); return; } - - StatusCode response = motor.setPosition(rotations.getAsDouble()); - if (response.isOK()) { - return; - } - - response = motor.setPosition(rotations.getAsDouble()); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setPosition(rotations.getAsDouble())); } /** @@ -380,15 +385,7 @@ protected void setMMVelocityFOC(DoubleSupplier velocityRPS) { MotionMagicVelocityTorqueCurrentFOC mm = config.mmVelocityFOC.withVelocity(velocityRPS.getAsDouble()); - StatusCode response = motor.setControl(mm); - if (response.isOK()) { - return; - } - - response = motor.setControl(mm); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(mm)); } /** @@ -404,15 +401,7 @@ protected void setVelocityTorqueCurrentFOC(DoubleSupplier velocityRPS) { VelocityTorqueCurrentFOC output = config.velocityTorqueCurrentFOC.withVelocity(velocityRPS.getAsDouble()); - StatusCode response = motor.setControl(output); - if (response.isOK()) { - return; - } - - response = motor.setControl(output); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(output)); } /** @@ -429,15 +418,7 @@ protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPS) { config.velocityTorqueCurrentFOC.withVelocity( Conversions.RPMtoRPS(velocityRPS.getAsDouble())); - StatusCode response = motor.setControl(output); - if (response.isOK()) { - return; - } - - response = motor.setControl(output); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(output)); } /** @@ -452,15 +433,7 @@ protected void setVelocity(DoubleSupplier velocityRPS) { VelocityVoltage output = config.velocityControl.withVelocity(velocityRPS.getAsDouble()); - StatusCode response = motor.setControl(output); - if (response.isOK()) { - return; - } - - response = motor.setControl(output); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(output)); } /** @@ -475,15 +448,7 @@ protected void setMMPositionFoc(DoubleSupplier rotations) { MotionMagicTorqueCurrentFOC mm = config.mmPositionFOC.withPosition(rotations.getAsDouble()); - StatusCode response = motor.setControl(mm); - if (response.isOK()) { - return; - } - - response = motor.setControl(mm); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(mm)); } /** @@ -509,15 +474,7 @@ public void setMMPosition(DoubleSupplier rotations, int slot) { MotionMagicVoltage mm = config.mmPositionVoltageSlot.withSlot(slot).withPosition(rotations.getAsDouble()); - StatusCode response = motor.setControl(mm); - if (response.isOK()) { - return; - } - - response = motor.setControl(mm); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(mm)); } /** @@ -534,15 +491,7 @@ public void setPercentOutput(DoubleSupplier percent) { config.voltageControl.withOutput( config.voltageCompSaturation * percent.getAsDouble()); - StatusCode response = motor.setControl(output); - if (response.isOK()) { - return; - } - - response = motor.setControl(output); - if (!response.isOK()) { - motorDisconnectedAlert.set(true); - } + checkMotorOK(() -> motor.setControl(output)); } public void setBrakeMode(boolean isInBrake) { From c7a31138e27d3e0551ff29ede10aafc4a13303fa Mon Sep 17 00:00:00 2001 From: cycIes Date: Sat, 21 Dec 2024 16:07:02 -0600 Subject: [PATCH 3/3] check if the motor is licensed when motor is caleld --- .../frc/spectrumLib/mechanism/Mechanism.java | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java index 0154adac..1f615b5d 100644 --- a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java +++ b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java @@ -47,8 +47,9 @@ public abstract class Mechanism implements NTSendable, SpectrumSubsystem { public Config config; Alert currentAlert = new Alert("", AlertType.kWarning); - Alert motorDisconnectedAlert = + Alert disconnectedAlert = new Alert(getName() + " motor " + config.getId() + " disconnected", AlertType.kWarning); + Alert unlicensedAlert = new Alert(getName() + " motor " + config.getId() + " unlicensed", AlertType.kWarning); private final CachedDouble cachedRotations; private final CachedDouble cachedPercentage; @@ -350,12 +351,19 @@ protected void tareMotor() { protected void checkMotorOK(Supplier motorRequest) { if (motorRequest.get().isOK()) { - motorDisconnectedAlert.set(false); + disconnectedAlert.set(false); + unlicensedAlert.set(false); return; } - if (!motorRequest.get().isOK()) { - motorDisconnectedAlert.set(true); + if (motorRequest.get().isOK()) { + return; + } + + disconnectedAlert.set(true); + + if (motorRequest.get().value == StatusCode.UnlicensedDevice.value) { + unlicensedAlert.set(true); } } @@ -366,7 +374,7 @@ protected void checkMotorOK(Supplier motorRequest) { */ protected void setMotorPosition(DoubleSupplier rotations) { if (!isAttached()) { - motorDisconnectedAlert.set(false); + disconnectedAlert.set(false); return; } checkMotorOK(() -> motor.setPosition(rotations.getAsDouble()));