diff --git a/.vscode/extensions.json b/.vscode/extensions.json index a3a224e7..5056e03c 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -2,6 +2,7 @@ "recommendations": [ "richardwillis.vscode-spotless-gradle", "sonarsource.sonarlint-vscode", - "streetsidesoftware.code-spell-checker" + "streetsidesoftware.code-spell-checker", + "onlyutkarsh.git-config-user-profiles" ] } \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 8a031736..c9f593f2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -65,6 +65,7 @@ "rezero", "scurve", "Sedgewick", + "selfcheck", "setpoint", "setpoints", "SIMBOT", diff --git a/README.md b/README.md index 308aeda0..d1b417b5 100644 --- a/README.md +++ b/README.md @@ -121,3 +121,4 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can * AdvantageKit-enabled pneumatics classes from Mechanical Advantage's 2022 [robot code](https://github.com/Mechanical-Advantage/RobotCode2022) * FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from [Ranger Robotics](https://github.com/3015RangerRobotics/2023Public) * Talon factories from Citrus Circuits 2022 [robot code](https://github.com/frc1678/C2022) +* FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from Ranger Robotics 2023 [robot code](https://github.com/3015RangerRobotics/2023Public) diff --git a/networktables.json b/networktables.json new file mode 100644 index 00000000..fe51488c --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui.json b/simgui.json index 01556c05..6d612087 100644 --- a/simgui.json +++ b/simgui.json @@ -15,6 +15,17 @@ "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/Shuffleboard/MAIN/SendableChooser[0]": "String Chooser", + "/Shuffleboard/Subsystem/Subsystem": "Subsystem", + "/SmartDashboard//SystemStatus/Drivetrain/SystemCheck": "Command", + "/SmartDashboard//SystemStatus/SwerveModule0/SystemCheck": "Command", + "/SmartDashboard//SystemStatus/SwerveModule1/SystemCheck": "Command", + "/SmartDashboard//SystemStatus/SwerveModule2/SystemCheck": "Command", + "/SmartDashboard//SystemStatus/SwerveModule3/SystemCheck": "Command", + "/SmartDashboard//SystemStatusDrivetrain/SystemCheck": "Command", + "/SmartDashboard//SystemStatusSwerveModule0/SystemCheck": "Command", + "/SmartDashboard//SystemStatusSwerveModule1/SystemCheck": "Command", + "/SmartDashboard//SystemStatusSwerveModule2/SystemCheck": "Command", + "/SmartDashboard//SystemStatusSwerveModule3/SystemCheck": "Command", "/SmartDashboard/Auto Routine": "String Chooser", "/SmartDashboard/PPSwerveControllerCommand_field": "Field2d", "/SmartDashboard/simCamera Sim Field": "Field2d" diff --git a/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java b/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java new file mode 100644 index 00000000..7e89cec2 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java @@ -0,0 +1,234 @@ +package frc.lib.team3015.subsystem; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.lib.team3015.subsystem.selfcheck.*; +import frc.lib.team6328.util.Alert; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import org.littletonrobotics.junction.Logger; + +// derived from 3015's AdvancedSubsystem abstract class + +public class FaultReporter { + public enum SystemStatus { + OK, + WARNING, + ERROR + } + + private static class SubsystemFaults { + private List faults = new ArrayList<>(); + private List faultAlerts = new ArrayList<>(); + private List hardware = new ArrayList<>(); + } + + private static FaultReporter instance = null; + + private static final String CHECK_RAN = "/CheckRan"; + private static final String SYSTEM_STATUS = "SystemStatus/"; + + private final Map subsystemsFaults = new HashMap<>(); + private final boolean checkErrors; + + private FaultReporter() { + this.checkErrors = RobotBase.isReal(); + setupCallbacks(); + } + + public static FaultReporter getInstance() { + if (instance == null) { + instance = new FaultReporter(); + } + return instance; + } + + public CommandBase registerSystemCheck(String subsystemName, CommandBase systemCheckCommand) { + String statusTable = SYSTEM_STATUS + subsystemName; + SubsystemFaults subsystemFaults = + subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); + + CommandBase wrappedSystemCheckCommand = + wrapSystemCheckCommand(subsystemName, systemCheckCommand); + wrappedSystemCheckCommand.setName(subsystemName + "Check"); + SmartDashboard.putData(statusTable + "/SystemCheck", wrappedSystemCheckCommand); + Logger.getInstance().recordOutput(statusTable + CHECK_RAN, false); + + subsystemsFaults.put(subsystemName, subsystemFaults); + + return wrappedSystemCheckCommand; + } + + private CommandBase wrapSystemCheckCommand(String subsystemName, CommandBase systemCheckCommand) { + String statusTable = SYSTEM_STATUS + subsystemName; + return Commands.sequence( + Commands.runOnce( + () -> { + Logger.getInstance().recordOutput(statusTable + CHECK_RAN, false); + clearFaults(subsystemName); + publishStatus(); + }), + systemCheckCommand, + Commands.runOnce( + () -> { + publishStatus(); + Logger.getInstance().recordOutput(statusTable + CHECK_RAN, true); + })); + } + + private void setupCallbacks() { + CommandScheduler.getInstance() + .schedule( + Commands.repeatingSequence( + Commands.runOnce(this::checkForFaults), Commands.waitSeconds(0.25)) + .ignoringDisable(true)); + + CommandScheduler.getInstance() + .schedule( + Commands.repeatingSequence( + Commands.runOnce(this::publishStatus), Commands.waitSeconds(1.0)) + .ignoringDisable(true)); + } + + private void publishStatus() { + for (Map.Entry entry : subsystemsFaults.entrySet()) { + String subsystemName = entry.getKey(); + SubsystemFaults subsystemFaults = entry.getValue(); + + SystemStatus status = getSystemStatus(subsystemFaults.faults); + + String statusTable = SYSTEM_STATUS + subsystemName; + Logger.getInstance().recordOutput(statusTable + "/Status", status.name()); + Logger.getInstance().recordOutput(statusTable + "/SystemOK", status == SystemStatus.OK); + + String[] faultStrings = new String[subsystemFaults.faults.size()]; + for (int i = 0; i < subsystemFaults.faults.size(); i++) { + SubsystemFault fault = subsystemFaults.faults.get(i); + faultStrings[i] = String.format("[%.2f] %s", fault.timestamp, fault.description); + } + Logger.getInstance().recordOutput(statusTable + "/Faults", faultStrings); + + if (faultStrings.length > 0) { + Logger.getInstance() + .recordOutput(statusTable + "/LastFault", faultStrings[faultStrings.length - 1]); + } else { + Logger.getInstance().recordOutput(statusTable + "/LastFault", ""); + } + } + } + + public void addFault(String subsystemName, SubsystemFault fault) { + SubsystemFaults subsystems = subsystemsFaults.get(subsystemName); + List subsystemFaults = subsystems.faults; + List subsystemAlerts = subsystems.faultAlerts; + if (!subsystemFaults.contains(fault)) { + subsystemFaults.add(fault); + + Alert alert = + new Alert( + subsystemName + ": " + fault.description, + fault.isWarning ? Alert.AlertType.WARNING : Alert.AlertType.ERROR); + alert.set(true); + subsystemAlerts.add(alert); + } + } + + public void addFault(String subsystemName, String description, boolean isWarning) { + this.addFault(subsystemName, new SubsystemFault(description, isWarning)); + } + + public void addFault( + String subsystemName, String description, boolean isWarning, boolean sticky) { + this.addFault(subsystemName, new SubsystemFault(description, isWarning, sticky)); + } + + public void addFault(String subsystemName, String description) { + this.addFault(subsystemName, description, false); + } + + public List getFaults(String subsystemName) { + return subsystemsFaults.get(subsystemName).faults; + } + + public void clearFaults(String subsystemName) { + subsystemsFaults.get(subsystemName).faults.clear(); + } + + private SystemStatus getSystemStatus(List subsystemFaults) { + SystemStatus worstStatus = SystemStatus.OK; + + for (SubsystemFault f : subsystemFaults) { + if (f.sticky || f.timestamp > Timer.getFPGATimestamp() - 10) { + if (f.isWarning) { + if (worstStatus != SystemStatus.ERROR) { + worstStatus = SystemStatus.WARNING; + } + } else { + worstStatus = SystemStatus.ERROR; + } + } + } + return worstStatus; + } + + public void registerHardware(String subsystemName, String label, TalonFX phoenixMotor) { + SubsystemFaults subsystemFaults = + subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); + subsystemFaults.hardware.add(new SelfCheckingPhoenixMotor(label, phoenixMotor)); + subsystemsFaults.put(subsystemName, subsystemFaults); + } + + public void registerHardware(String subsystemName, String label, PWMMotorController pwmMotor) { + SubsystemFaults subsystemFaults = + subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); + subsystemFaults.hardware.add(new SelfCheckingPWMMotor(label, pwmMotor)); + subsystemsFaults.put(subsystemName, subsystemFaults); + } + + public void registerHardware(String subsystemName, String label, CANSparkMax spark) { + SubsystemFaults subsystemFaults = + subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); + subsystemFaults.hardware.add(new SelfCheckingSparkMax(label, spark)); + subsystemsFaults.put(subsystemName, subsystemFaults); + } + + public void registerHardware(String subsystemName, String label, Pigeon2 pigeon2) { + SubsystemFaults subsystemFaults = + subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); + subsystemFaults.hardware.add(new SelfCheckingPigeon2(label, pigeon2)); + subsystemsFaults.put(subsystemName, subsystemFaults); + } + + public void registerHardware(String subsystemName, String label, CANcoder canCoder) { + SubsystemFaults subsystemFaults = + subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); + subsystemFaults.hardware.add(new SelfCheckingCANCoder(label, canCoder)); + subsystemsFaults.put(subsystemName, subsystemFaults); + } + + // Method to check for faults while the robot is operating normally + private void checkForFaults() { + if (checkErrors) { + for (Map.Entry entry : subsystemsFaults.entrySet()) { + String subsystemName = entry.getKey(); + SubsystemFaults subsystemFaults = entry.getValue(); + for (SelfChecking device : subsystemFaults.hardware) { + for (SubsystemFault fault : device.checkForFaults()) { + addFault(subsystemName, fault); + } + } + } + } + } +} diff --git a/src/main/java/frc/lib/team3015/subsystem/SubsystemFault.java b/src/main/java/frc/lib/team3015/subsystem/SubsystemFault.java new file mode 100644 index 00000000..f4749708 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/SubsystemFault.java @@ -0,0 +1,52 @@ +package frc.lib.team3015.subsystem; + +import edu.wpi.first.wpilibj.Timer; +import java.util.Objects; + +public class SubsystemFault { + public final String description; + public final double timestamp; + public final boolean isWarning; + public final boolean sticky; + + public SubsystemFault(String description, boolean isWarning) { + // default sticky to true + this(description, isWarning, true); + } + + public SubsystemFault(String description) { + this(description, false); + } + + public SubsystemFault(String description, boolean isWarning, boolean sticky) { + this.description = description; + this.timestamp = Timer.getFPGATimestamp(); + this.isWarning = isWarning; + this.sticky = sticky; + } + + @Override + public boolean equals(Object other) { + if (this == other) { + return true; + } + + if (other == null) { + return false; + } + + if (getClass() != other.getClass()) { + return false; + } + + SubsystemFault otherSubsystemFault = (SubsystemFault) other; + + return description.equals(otherSubsystemFault.description) + && isWarning == otherSubsystemFault.isWarning; + } + + @Override + public int hashCode() { + return Objects.hash(description, timestamp, isWarning, sticky); + } +} diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfChecking.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfChecking.java new file mode 100644 index 00000000..62e2b361 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfChecking.java @@ -0,0 +1,8 @@ +package frc.lib.team3015.subsystem.selfcheck; + +import frc.lib.team3015.subsystem.SubsystemFault; +import java.util.List; + +public interface SelfChecking { + List checkForFaults(); +} diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java new file mode 100644 index 00000000..0a2fc154 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java @@ -0,0 +1,40 @@ +package frc.lib.team3015.subsystem.selfcheck; + +import com.ctre.phoenix6.hardware.CANcoder; +import frc.lib.team3015.subsystem.SubsystemFault; +import java.util.ArrayList; +import java.util.List; + +public class SelfCheckingCANCoder implements SelfChecking { + private final String label; + private final CANcoder canCoder; + + public SelfCheckingCANCoder(String label, CANcoder canCoder) { + this.label = label; + this.canCoder = canCoder; + } + + @Override + public List checkForFaults() { + List faults = new ArrayList<>(); + + if (canCoder.getFault_Hardware().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Hardware fault detected", label))); + } + if (canCoder.getFault_BootDuringEnable().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Device booted while enabled", label))); + } + if (canCoder.getFault_BadMagnet().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Bad magnet", label))); + } + if (canCoder.getFault_Undervoltage().getValue()) { + faults.add( + new SubsystemFault(String.format("[%s]: Device supply voltage near brownout", label))); + } + if (canCoder.getFault_UnlicensedFeatureInUse().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Unlicensed feature in use", label))); + } + + return faults; + } +} diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPWMMotor.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPWMMotor.java new file mode 100644 index 00000000..228a3659 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPWMMotor.java @@ -0,0 +1,27 @@ +package frc.lib.team3015.subsystem.selfcheck; + +import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; +import frc.lib.team3015.subsystem.SubsystemFault; +import java.util.ArrayList; +import java.util.List; + +public class SelfCheckingPWMMotor implements SelfChecking { + private final String label; + private final PWMMotorController motor; + + public SelfCheckingPWMMotor(String label, PWMMotorController motor) { + this.label = label; + this.motor = motor; + } + + @Override + public List checkForFaults() { + List faults = new ArrayList<>(); + + if (!motor.isAlive()) { + faults.add(new SubsystemFault(String.format("[%s]: Device timed out", label))); + } + + return faults; + } +} diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java new file mode 100644 index 00000000..c18076a1 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPhoenixMotor.java @@ -0,0 +1,58 @@ +package frc.lib.team3015.subsystem.selfcheck; + +import com.ctre.phoenix6.hardware.TalonFX; +import frc.lib.team3015.subsystem.SubsystemFault; +import java.util.ArrayList; +import java.util.List; + +public class SelfCheckingPhoenixMotor implements SelfChecking { + private final String label; + private final TalonFX motor; + + public SelfCheckingPhoenixMotor(String label, TalonFX motor) { + this.label = label; + this.motor = motor; + } + + @Override + public List checkForFaults() { + List faults = new ArrayList<>(); + + if (motor.getFault_Hardware().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Hardware failure detected", label))); + } + if (motor.getFault_BootDuringEnable().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Device booted while enabled", label))); + } + if (motor.getFault_DeviceTemp().getValue()) { + faults.add( + new SubsystemFault( + String.format("[%s]: Device temperature exceeded limit", label), true)); + } + if (motor.getFault_FusedSensorOutOfSync().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Remote sensor is out of sync", label))); + } + if (motor.getFault_MissingRemoteSensor().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Remote sensor is missing", label))); + } + if (motor.getFault_OverSupplyV().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Supply voltage exceeded limit", label))); + } + if (motor.getFault_ProcTemp().getValue()) { + faults.add( + new SubsystemFault(String.format("[%s]: Processor temperature exceeded limit", label))); + } + if (motor.getFault_Undervoltage().getValue()) { + faults.add( + new SubsystemFault(String.format("[%s]: Device supply voltage near brownout", label))); + } + if (motor.getFault_UnlicensedFeatureInUse().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Unlicensed feature in use", label))); + } + if (motor.getFault_UnstableSupplyV().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Supply voltage is unstable", label))); + } + + return faults; + } +} diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java new file mode 100644 index 00000000..9cbaafa9 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPigeon2.java @@ -0,0 +1,73 @@ +package frc.lib.team3015.subsystem.selfcheck; + +import com.ctre.phoenix6.hardware.Pigeon2; +import frc.lib.team3015.subsystem.SubsystemFault; +import java.util.ArrayList; +import java.util.List; + +public class SelfCheckingPigeon2 implements SelfChecking { + private final String label; + private final Pigeon2 pigeon; + + public SelfCheckingPigeon2(String label, Pigeon2 pigeon) { + this.label = label; + this.pigeon = pigeon; + } + + @Override + public List checkForFaults() { + List faults = new ArrayList<>(); + + if (pigeon.getFault_Hardware().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Hardware fault detected", label))); + } + if (pigeon.getFault_BootDuringEnable().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Device booted while enabled", label))); + } + if (pigeon.getFault_BootIntoMotion().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Device booted while in motion", label))); + } + if (pigeon.getFault_BootupGyroscope().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Gyro fault detected", label))); + } + if (pigeon.getFault_BootupAccelerometer().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Accelerometer fault detected", label))); + } + if (pigeon.getFault_BootupMagnetometer().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Magnetometer fault detected", label))); + } + if (pigeon.getFault_DataAcquiredLate().getValue()) { + faults.add( + new SubsystemFault( + String.format("[%s]: Motion stack data acquisition slower than expected", label))); + } + if (pigeon.getFault_Hardware().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Hardware failure", label))); + } + if (pigeon.getFault_LoopTimeSlow().getValue()) { + faults.add( + new SubsystemFault( + String.format("[%s]: Motion stack loop time was slower than expected", label))); + } + if (pigeon.getFault_SaturatedAccelometer().getValue()) { + faults.add( + new SubsystemFault(String.format("[%s]: Accelerometer values are saturated", label))); + } + if (pigeon.getFault_SaturatedGyrosscope().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Gyro values are saturated", label))); + } + if (pigeon.getFault_SaturatedMagneter().getValue()) { + faults.add( + new SubsystemFault(String.format("[%s]: Magnetometer values are saturated", label))); + } + if (pigeon.getFault_Undervoltage().getValue()) { + faults.add( + new SubsystemFault(String.format("[%s]: Device supply voltage near brownout", label))); + } + if (pigeon.getFault_UnlicensedFeatureInUse().getValue()) { + faults.add(new SubsystemFault(String.format("[%s]: Unlicensed feature in use", label))); + } + + return faults; + } +} diff --git a/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingSparkMax.java b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingSparkMax.java new file mode 100644 index 00000000..66048a00 --- /dev/null +++ b/src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingSparkMax.java @@ -0,0 +1,29 @@ +package frc.lib.team3015.subsystem.selfcheck; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import frc.lib.team3015.subsystem.SubsystemFault; +import java.util.ArrayList; +import java.util.List; + +public class SelfCheckingSparkMax implements SelfChecking { + private final String label; + private CANSparkMax spark; + + public SelfCheckingSparkMax(String label, CANSparkMax spark) { + this.label = label; + this.spark = spark; + } + + @Override + public List checkForFaults() { + ArrayList faults = new ArrayList<>(); + + REVLibError err = spark.getLastError(); + if (err != REVLibError.kOk) { + faults.add(new SubsystemFault(String.format("[%s]: Error: %s", label, err.name()))); + } + + return faults; + } +} diff --git a/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java b/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java index 783c3462..b78d35c8 100644 --- a/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java +++ b/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.sim.Pigeon2SimState; import edu.wpi.first.wpilibj.RobotController; +import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; import frc.robot.Constants; import java.util.ArrayList; @@ -40,6 +41,8 @@ public GyroIOPigeon2Phoenix6(int id) { this.angularVelocityZStatusSignal = this.gyro.getAngularVelocityZ(); this.angularVelocityZStatusSignal.setUpdateFrequency(100); + FaultReporter.getInstance().registerHardware("Drivetrain", "gyro", gyro); + if (Constants.getMode() == Constants.Mode.SIM) { this.gyroSim = this.gyro.getSimState(); } else { diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java index d1d83964..3d46bd33 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java @@ -12,6 +12,9 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.lib.team3015.subsystem.FaultReporter; import java.util.List; import org.littletonrobotics.junction.Logger; @@ -21,11 +24,12 @@ public class SwerveModule { private final SwerveModuleIOInputsAutoLogged inputs = new SwerveModuleIOInputsAutoLogged(); private int moduleNumber; + private String subsystemName; private double lastAngle; private double maxVelocity; private double lastAngleMotorVelocity = 0.0; + private CommandBase wrappedSystemCheckCommand; - private static final String SUBSYSTEM_NAME = "Swerve"; private static final boolean DEBUGGING = false; /** @@ -39,18 +43,23 @@ public SwerveModule(SwerveModuleIO io, int moduleNumber, double maxVelocity) { this.io = io; this.moduleNumber = moduleNumber; this.maxVelocity = maxVelocity; + this.subsystemName = "SwerveModule" + moduleNumber; lastAngle = getState().angle.getDegrees(); /* set DEBUGGING to true to view values in Shuffleboard. This is useful when determining the steer offset constants. */ if (DEBUGGING) { - ShuffleboardTab tab = Shuffleboard.getTab(SUBSYSTEM_NAME); + ShuffleboardTab tab = Shuffleboard.getTab("Swerve"); tab.addNumber( "Mod " + this.moduleNumber + ": Cancoder", () -> inputs.angleAbsolutePositionDeg); tab.addNumber("Mod " + this.moduleNumber + ": Integrated", () -> inputs.anglePositionDeg); tab.addNumber( "Mod " + this.moduleNumber + ": Velocity", () -> inputs.driveVelocityMetersPerSec); } + + FaultReporter faultReporter = FaultReporter.getInstance(); + this.wrappedSystemCheckCommand = + faultReporter.registerSystemCheck(this.subsystemName, getSystemCheckCommand()); } /** @@ -182,6 +191,59 @@ public void setAngleBrakeMode(boolean enable) { io.setAngleBrakeMode(enable); } + private CommandBase getSystemCheckCommand() { + return Commands.sequence( + Commands.run(() -> io.setAnglePosition(90.0)).withTimeout(1.0), + Commands.runOnce( + () -> { + double angle = inputs.anglePositionDeg % 360; + if (angle < 88 || angle > 92) { + FaultReporter.getInstance() + .addFault( + this.subsystemName, + "[System Check] Rotation Motor did not reach target position of 90 deg " + + angle, + false, + true); + } + }), + Commands.runOnce(() -> io.setDriveMotorPercentage(0.1)), + Commands.waitSeconds(0.5), + Commands.runOnce( + () -> { + if (inputs.driveVelocityMetersPerSec < 0.25) { + FaultReporter.getInstance() + .addFault( + this.subsystemName, + "[System Check] Drive motor encoder velocity too slow", + false, + true); + } + io.setDriveMotorPercentage(0.0); + }), + Commands.waitSeconds(0.25), + Commands.run(() -> io.setAnglePosition(0.0)).withTimeout(1.0), + Commands.runOnce( + () -> { + double angle = inputs.anglePositionDeg % 360; + if (angle < -2 || angle > 2) { + FaultReporter.getInstance() + .addFault( + this.subsystemName, + "[System Check] Rotation Motor did not reach target position of 0 deg " + + angle, + false, + true); + } + })) + .until(() -> !FaultReporter.getInstance().getFaults(this.subsystemName).isEmpty()) + .andThen(Commands.runOnce(() -> io.setDriveMotorPercentage(0.0))); + } + + public CommandBase getCheckCommand() { + return this.wrappedSystemCheckCommand; + } + /** * Get the velocity of the angle motor in radians per second. * diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java index 4da44ef3..40df1416 100644 --- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java +++ b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFXPhoenix6.java @@ -24,11 +24,11 @@ import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; import frc.lib.team6328.util.Alert; import frc.lib.team6328.util.Alert.AlertType; @@ -93,7 +93,8 @@ public class SwerveModuleIOTalonFXPhoenix6 implements SwerveModuleIO { private TalonFXSimState driveMotorSimState; private CANcoderSimState angleEncoderSimState; private LinearSystemSim driveSim; - private FlywheelSim turnSim; + private LinearSystemSim turnSim; + private double lastSimAnglePositionRot = 0.0; /** * Make a new SwerveModuleIOTalonFX object. @@ -131,6 +132,11 @@ public SwerveModuleIOTalonFXPhoenix6( configAngleMotor(angleMotorID, canCoderID); configDriveMotor(driveMotorID); configSim(); + + String subsystemName = "SwerveModule" + moduleNumber; + FaultReporter.getInstance().registerHardware(subsystemName, "angle encoder", angleEncoder); + FaultReporter.getInstance().registerHardware(subsystemName, "angle motor", angleMotor); + FaultReporter.getInstance().registerHardware(subsystemName, "drive motor", driveMotor); } private void configAngleEncoder(int canCoderID) { @@ -415,9 +421,12 @@ private void configSim() { ? ChassisReference.Clockwise_Positive : ChassisReference.CounterClockwise_Positive; - // replace the flywheel sim with the position system after characterizing the rotation of the - // swerve modules on the robot - this.turnSim = new FlywheelSim(DCMotor.getFalcon500(1), angleGearRatio, 0.5); + this.turnSim = + new LinearSystemSim<>( + LinearSystemId.identifyPositionSystem( + RobotConfig.getInstance().getSwerveAngleKV(), + RobotConfig.getInstance().getSwerveAngleKA())); + this.driveSim = new LinearSystemSim<>( LinearSystemId.identifyVelocitySystem( @@ -435,7 +444,7 @@ private void updateSim() { this.driveMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); // update the input voltages of the models based on the outputs of the simulated TalonFXs - this.turnSim.setInputVoltage(this.angleMotorSimState.getMotorVoltage()); + this.turnSim.setInput(this.angleMotorSimState.getMotorVoltage()); this.driveSim.setInput(this.driveMotorSimState.getMotorVoltage()); // update the models @@ -443,11 +452,13 @@ private void updateSim() { this.turnSim.update(Constants.LOOP_PERIOD_SECS); // update the simulated TalonFXs and CANcoder based on the model outputs - double turnRPM = turnSim.getAngularVelocityRPM(); - double angleEncoderRPS = turnRPM / 60.0; - double angleEncoderRotations = angleEncoderRPS * Constants.LOOP_PERIOD_SECS; + double turnRadians = turnSim.getOutput(0); + double angleEncoderRotations = turnRadians / (2 * Math.PI); + double angleEncoderRPS = + (angleEncoderRotations - lastSimAnglePositionRot) / Constants.LOOP_PERIOD_SECS; + lastSimAnglePositionRot = angleEncoderRotations; - this.angleEncoderSimState.addPosition(angleEncoderRotations); + this.angleEncoderSimState.setRawPosition(angleEncoderRotations); this.angleEncoderSimState.setVelocity(angleEncoderRPS); this.angleMotorSimState.addRotorPosition(angleEncoderRotations / angleGearRatio); this.angleMotorSimState.setRotorVelocity(angleEncoderRPS / angleGearRatio); diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java index 1b22126a..217f867d 100644 --- a/src/main/java/frc/robot/commands/DriveToPose.java +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -154,6 +154,7 @@ public void execute() { // the calculate method has not yet been invoked. running = true; + // Update from tunable numbers if (driveKp.hasChanged() || driveKd.hasChanged() diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 56cfbc74..5273f6ea 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -24,8 +24,11 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; import frc.lib.team3061.gyro.GyroIO; import frc.lib.team3061.gyro.GyroIOInputsAutoLogged; @@ -136,6 +139,8 @@ public class Drivetrain extends SubsystemBase { private boolean isMoveToPoseEnabled; + private double initialGyroPositionForSystemCheck = 0.0; + private Alert noPoseAlert = new Alert("Attempted to reset pose from vision, but no pose was found.", AlertType.WARNING); @@ -211,6 +216,9 @@ public Drivetrain( tab.add("Enable XStance", new InstantCommand(this::enableXstance)); tab.add("Disable XStance", new InstantCommand(this::disableXstance)); } + + FaultReporter faultReporter = FaultReporter.getInstance(); + faultReporter.registerSystemCheck(SUBSYSTEM_NAME, getSystemCheckCommand()); } /** Enables "turbo" mode (i.e., acceleration is not limited by software) */ @@ -837,6 +845,17 @@ public boolean isMoveToPoseEnabled() { return this.isMoveToPoseEnabled; } + private CommandBase getSystemCheckCommand() { + return Commands.sequence( + Commands.sequence( + swerveModules[0].getCheckCommand(), + swerveModules[1].getCheckCommand(), + swerveModules[2].getCheckCommand(), + swerveModules[3].getCheckCommand())) + .until(() -> !FaultReporter.getInstance().getFaults(SUBSYSTEM_NAME).isEmpty()) + .andThen(Commands.runOnce(() -> drive(0, 0, 0, true, false), this)); + } + /** * If the robot is enabled and brake mode is not enabled, enable it. If the robot is disabled, has * stopped moving for the specified period of time, and brake mode is enabled, disable it. diff --git a/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java b/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java index 152fc173..4110f868 100644 --- a/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java @@ -4,7 +4,10 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team6328.util.TunableNumber; import org.littletonrobotics.junction.Logger; @@ -39,6 +42,8 @@ public Subsystem(SubsystemIO io) { ShuffleboardTab tab = Shuffleboard.getTab(SUBSYSTEM_NAME); tab.add(SUBSYSTEM_NAME, this); } + + FaultReporter.getInstance().registerSystemCheck(SUBSYSTEM_NAME, getSystemCheckCommand()); } /** @@ -92,4 +97,34 @@ public void setMotorCurrent(double power) { public void setMotorPosition(double position) { io.setMotorPosition(position, POSITION_FEEDFORWARD); } + + private CommandBase getSystemCheckCommand() { + return Commands.sequence( + Commands.run(() -> io.setMotorPower(0.3)).withTimeout(1.0), + Commands.runOnce( + () -> { + if (inputs.velocityRPM < 2.0) { + FaultReporter.getInstance() + .addFault( + SUBSYSTEM_NAME, + "[System Check] Subsystem motor not moving as fast as expected", + false, + true); + } + }), + Commands.run(() -> io.setMotorPower(-0.2)).withTimeout(1.0), + Commands.runOnce( + () -> { + if (inputs.velocityRPM > -2.0) { + FaultReporter.getInstance() + .addFault( + SUBSYSTEM_NAME, + "[System Check] Subsystem motor moving too slow or in the wrong direction", + false, + true); + } + })) + .until(() -> !FaultReporter.getInstance().getFaults(SUBSYSTEM_NAME).isEmpty()) + .andThen(Commands.runOnce(() -> io.setMotorPower(0.0))); + } } diff --git a/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java index 0db2b0a0..00bd6b06 100644 --- a/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.lib.team3015.subsystem.FaultReporter; import frc.lib.team3061.RobotConfig; import frc.lib.team3061.swerve.Conversions; import frc.lib.team6328.util.Alert; @@ -149,5 +150,7 @@ private void configMotor(int motorID) { this.currentRequest = new TorqueCurrentFOC(0.0); this.positionRequest = new PositionVoltage(0.0).withSlot(0); this.positionRequest.EnableFOC = RobotConfig.getInstance().getPhoenix6Licensed(); + + FaultReporter.getInstance().registerHardware(SUBSYSTEM_NAME, "subsystem motor", motor); } }