diff --git a/simgui-ds.json b/simgui-ds.json index 1dda01e2..54fc6b27 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -102,8 +102,9 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0", - "name": "Keyboard" + "guid": "78696e70757401000000000000000000", + "name": "Keyboard", + "useGamepad": true }, { "useGamepad": true diff --git a/simgui.json b/simgui.json index cf64b176..f9714fc2 100644 --- a/simgui.json +++ b/simgui.json @@ -12,20 +12,41 @@ "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d", "/FMSInfo": "FMSInfo", "/SmartDashboard/Auto Chooser": "String Chooser", + "/SmartDashboard/Blacklist All": "Command", + "/SmartDashboard/Blacklist Note": "Command", + "/SmartDashboard/Dynamic Auto": "Command", + "/SmartDashboard/Dynamic Demo": "Command", + "/SmartDashboard/Note Picker": "String Chooser", + "/SmartDashboard/Note To Note": "Command", + "/SmartDashboard/Note To Shooting": "Command", "/SmartDashboard/Run Elevator Sysid": "Command", "/SmartDashboard/Run Flywheel Sysid": "Command", "/SmartDashboard/Run Pivot Sysid": "Command", "/SmartDashboard/Run Swerve Azimuth Sysid": "Command", "/SmartDashboard/Run Swerve Drive Sysid": "Command", + "/SmartDashboard/SendableChooser[0]": "String Chooser", + "/SmartDashboard/Set robot pose center": "Command", + "/SmartDashboard/Shoot To Note": "Command", "/SmartDashboard/Shooter shoot": "Command", + "/SmartDashboard/Start To Note": "Command", + "/SmartDashboard/Update Note": "Command", "/SmartDashboard/VisionSystemSim-Left Camera Sim System/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-Left Camera/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-Left_Camera/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-Right Camera Sim System/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-Right Camera/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-Right_Camera/Sim Field": "Field2d", - "/SmartDashboard/Zero shooter": "Command", - "/SmartDashboard/manual zero shooter": "Command" + "/SmartDashboard/Whitelist All": "Command", + "/SmartDashboard/Whitelist Note": "Command", + "/SmartDashboard/gleeb": "Command", + "/SmartDashboard/gloob": "Command" + }, + "windows": { + "/SmartDashboard/Auto Chooser": { + "window": { + "visible": true + } + } } }, "NetworkTables": { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a25bd933..f5b6c04b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,6 +50,7 @@ import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.logging.TalonFXFaultManager; import java.util.List; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.LogFileUtil; @@ -213,7 +214,9 @@ public void robotInit() { shooter.setDefaultCommand(shooter.runFlywheelsCmd(() -> 0.0, () -> 0.0)); leds.setDefaultCommand( leds.defaultStateDisplayCmd( - () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER)); + () -> DriverStation.isEnabled(), + () -> currentTarget == Target.SPEAKER, + () -> TalonFXFaultManager.getInstance().isOk())); controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); @@ -401,8 +404,6 @@ public void robotInit() { SmartDashboard.putData("Shooter shoot", shootWithDashboard()); SmartDashboard.putData("Run Swerve Azimuth Sysid", swerve.runModuleSteerCharacterizationCmd()); SmartDashboard.putData("Run Swerve Drive Sysid", swerve.runDriveCharacterizationCmd()); - SmartDashboard.putData("Run Elevator Sysid", elevator.runSysidCmd()); - SmartDashboard.putData("Run Pivot Sysid", shooter.runPivotSysidCmd()); SmartDashboard.putData("Run Flywheel Sysid", shooter.runFlywheelSysidCmd()); SmartDashboard.putData( "manual zero shooter", @@ -412,6 +413,8 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + // Update fault checker + TalonFXFaultManager.getInstance().periodic(); // Update ascope mechanism visualization Logger.recordOutput( "Mechanism Poses", diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java index 42b91cec..4a67c0af 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java @@ -4,16 +4,13 @@ package frc.robot.subsystems.carriage; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import org.littletonrobotics.junction.AutoLog; public interface CarriageIO { @AutoLog public static class CarriageIOInputs { - public double velocityRotationsPerSecond = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; - public double[] temperatureCelsius = new double[] {}; - // Not in cad as of 1/28 but requested + public TalonFXLog carriage = new TalonFXLog(0, 0, 0, 0, 0, 0); public boolean beambreak = false; } diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java index 8eabdc64..9765a977 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java @@ -4,26 +4,22 @@ package frc.robot.subsystems.carriage; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import frc.robot.utils.components.InvertedDigitalInput; +import frc.robot.utils.logging.TalonFXLogger; /** Create a CarriageIO that uses a real TalonFX. */ public class CarriageIOReal implements CarriageIO { - final TalonFX motor = new TalonFX(18, "canivore"); + private final TalonFX motor = new TalonFX(18, "canivore"); - final InvertedDigitalInput beambreak = new InvertedDigitalInput(2); + private final InvertedDigitalInput beambreak = new InvertedDigitalInput(2); - final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - final StatusSignal velocity = motor.getVelocity(); - final StatusSignal voltage = motor.getMotorVoltage(); - final StatusSignal amperage = motor.getStatorCurrent(); - final StatusSignal temp = motor.getDeviceTemp(); + private final TalonFXLogger logger = new TalonFXLogger(motor); public CarriageIOReal() { var config = new TalonFXConfiguration(); @@ -34,18 +30,12 @@ public CarriageIOReal() { config.CurrentLimits.SupplyCurrentLimitEnable = true; motor.getConfigurator().apply(config); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, amperage, temp); - motor.optimizeBusUtilization(); } /** Updates the set of loggable inputs. */ @Override public void updateInputs(final CarriageIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, amperage, temp); - inputs.velocityRotationsPerSecond = velocity.getValueAsDouble(); - inputs.appliedVolts = voltage.getValueAsDouble(); - inputs.currentAmps = new double[] {amperage.getValueAsDouble()}; - inputs.temperatureCelsius = new double[] {temp.getValueAsDouble()}; + inputs.carriage = logger.update(); inputs.beambreak = beambreak.get(); } diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java index bad032c2..bed9f157 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.logging.TalonFXFaultManager; import org.littletonrobotics.junction.Logger; /** Drainpipe style amp/trap mechanism on the elevator */ @@ -25,6 +26,7 @@ public CarriageSubsystem(CarriageIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Carriage", inputs); + TalonFXFaultManager.getInstance().addLog("Carriage", inputs.carriage); } /** Run the carriage roller at the specified voltage */ diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 12262702..dd4e16cb 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -10,6 +10,7 @@ public interface ClimberIO { @AutoLog public static class ClimberIOInputs { + // Not refactored bc removed public double climberVelocityRotationsPerSec = 0.0; public double climberAppliedVolts = 0.0; public double climberCurrentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index d6ad6bdf..f685028c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -4,16 +4,14 @@ package frc.robot.subsystems.elevator; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import org.littletonrobotics.junction.AutoLog; public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { - public double elevatorPositionMeters = 0.0; - public double elevatorVelocityMetersPerSec = 0.0; - public double elevatorAppliedVolts = 0.0; - public double[] elevatorCurrentAmps = new double[] {}; - public double[] elevatorTempCelsius = new double[] {}; + public TalonFXLog leader; + public TalonFXLog follower; } public void updateInputs(final ElevatorIOInputsAutoLogged inputs); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index cc5c529d..b9f8a004 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -4,8 +4,6 @@ package frc.robot.subsystems.elevator; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -15,6 +13,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.Servo; +import frc.robot.utils.logging.TalonFXLogger; /** Elevator IO using TalonFXs. */ public class ElevatorIOReal implements ElevatorIO { @@ -27,11 +26,9 @@ public class ElevatorIOReal implements ElevatorIO { private final MotionMagicVoltage positionVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true); - private final StatusSignal position = motor.getPosition(); - private final StatusSignal velocity = motor.getVelocity(); - private final StatusSignal voltage = motor.getMotorVoltage(); - private final StatusSignal current = motor.getStatorCurrent(); - private final StatusSignal temp = motor.getDeviceTemp(); + private final TalonFXLogger leaderLogger = new TalonFXLogger(motor); + + private final TalonFXLogger followerLogger = new TalonFXLogger(follower); public ElevatorIOReal() { var config = new TalonFXConfiguration(); @@ -64,20 +61,12 @@ public ElevatorIOReal() { motor.setPosition(0.0); // Assume we boot 0ed follower.getConfigurator().apply(config); follower.setControl(new Follower(motor.getDeviceID(), true)); - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, voltage, current, temp); - motor.optimizeBusUtilization(); - follower.optimizeBusUtilization(); } @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(position, velocity, voltage, current, temp); - inputs.elevatorPositionMeters = position.getValueAsDouble(); - inputs.elevatorVelocityMetersPerSec = velocity.getValueAsDouble(); - inputs.elevatorAppliedVolts = voltage.getValueAsDouble(); - inputs.elevatorCurrentAmps = new double[] {current.getValueAsDouble()}; - inputs.elevatorTempCelsius = new double[] {temp.getValueAsDouble()}; + inputs.leader = leaderLogger.update(); + inputs.follower = followerLogger.update(); } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 5804ea18..4eb40f7a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; /** WPILib physics model based elevator sim. */ public class ElevatorIOSim implements ElevatorIO { @@ -36,11 +37,14 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { stop(); } physicsSim.update(0.020); - inputs.elevatorPositionMeters = physicsSim.getPositionMeters(); - inputs.elevatorVelocityMetersPerSec = physicsSim.getVelocityMetersPerSecond(); - inputs.elevatorAppliedVolts = volts; - inputs.elevatorCurrentAmps = new double[] {physicsSim.getCurrentDrawAmps()}; - inputs.elevatorTempCelsius = new double[] {20.0}; + inputs.leader = + new TalonFXLog( + volts, + physicsSim.getCurrentDrawAmps(), + physicsSim.getCurrentDrawAmps(), + 0.0, + physicsSim.getPositionMeters(), + physicsSim.getVelocityMetersPerSecond()); } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 030de267..d601281d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -21,7 +21,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.utils.logging.TalonFXFaultManager; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -77,8 +77,10 @@ public ElevatorSubsystem(ElevatorIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); + TalonFXFaultManager.getInstance().addLog("Elevator Leader", inputs.leader); + TalonFXFaultManager.getInstance().addLog("Elevator Follower", inputs.follower); - carriage.setLength(inputs.elevatorPositionMeters); + carriage.setLength(inputs.leader.position); Logger.recordOutput("Elevator/Mechanism2d", mech2d); Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose()); @@ -109,40 +111,11 @@ public Command unlockClimb() { public Command runCurrentZeroing() { return this.run(() -> io.setVoltage(-1.0)) - .until(() -> inputs.elevatorCurrentAmps[0] > 40.0) + .until(() -> inputs.leader.statorCurrentAmps > 40.0) .finallyDo(() -> io.resetEncoder(0.0)) .beforeStarting(() -> io.setLockServoRotation(0.2)); } - public Command runSysidCmd() { - return Commands.sequence( - runCurrentZeroing(), - this.runOnce(() -> SignalLogger.start()), - // Stop when we get close to max to avoid hitting hard stop - elevatorRoutine - .quasistatic(Direction.kForward) - .until(() -> inputs.elevatorPositionMeters > MAX_EXTENSION_METERS - 0.2), - this.runOnce(() -> io.setVoltage(0.0)), - Commands.waitSeconds(1.0), - // Stop when we get close to max to avoid hitting hard stop - elevatorRoutine - .quasistatic(Direction.kReverse) - .until(() -> inputs.elevatorPositionMeters < 0.2), - this.runOnce(() -> io.setVoltage(0.0)), - Commands.waitSeconds(1.0), - // Stop when we get close to max to avoid hitting hard stop - elevatorRoutine - .dynamic(Direction.kForward) - .until(() -> inputs.elevatorPositionMeters > MAX_EXTENSION_METERS - 0.2), - this.runOnce(() -> io.setVoltage(0.0)), - Commands.waitSeconds(1.0), - // Stop when we get close to max to avoid hitting hard stop - elevatorRoutine - .dynamic(Direction.kReverse) - .until(() -> inputs.elevatorPositionMeters < 0.2), - this.runOnce(() -> SignalLogger.stop())); - } - public Pose3d getCarriagePose() { return new Pose3d( Units.inchesToMeters(4.5) + carriage.getLength() * ELEVATOR_ANGLE.getCos(), @@ -162,6 +135,6 @@ public Pose3d getFirstStagePose() { } public double getExtensionMeters() { - return inputs.elevatorPositionMeters; + return inputs.leader.position; } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index 176b70aa..74765709 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -4,15 +4,13 @@ package frc.robot.subsystems.feeder; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import org.littletonrobotics.junction.AutoLog; public interface FeederIO { @AutoLog public static class FeederIOInputs { - public double feederVelocityRotationsPerSec = 0.0; - public double feederAppliedVolts = 0.0; - public double feederCurrentAmps = 0.0; - public double feederTempC = 0.0; + public TalonFXLog feeder = new TalonFXLog(0, 0, 0, 0, 0, 0); public boolean firstBeambreak = false; public boolean lastBeambreak = false; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java index 51dc9a94..b7acbaa2 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -4,14 +4,13 @@ package frc.robot.subsystems.feeder; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import frc.robot.utils.components.InvertedDigitalInput; +import frc.robot.utils.logging.TalonFXLogger; /** Feeder IO using a TalonFX. */ public class FeederIOReal implements FeederIO { @@ -20,10 +19,7 @@ public class FeederIOReal implements FeederIO { private final InvertedDigitalInput firstBeambreak = new InvertedDigitalInput(0); private final InvertedDigitalInput lastBeambreak = new InvertedDigitalInput(1); - private final StatusSignal velocity = motor.getVelocity(); - private final StatusSignal voltage = motor.getMotorVoltage(); - private final StatusSignal current = motor.getStatorCurrent(); - private final StatusSignal temp = motor.getDeviceTemp(); + private final TalonFXLogger logger = new TalonFXLogger(motor); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); @@ -40,19 +36,11 @@ public FeederIOReal() { config.Slot0.kP = 0.1; motor.getConfigurator().apply(config); - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, current, temp); - motor.optimizeBusUtilization(); } @Override public void updateInputs(final FeederIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, current, temp); - - inputs.feederVelocityRotationsPerSec = velocity.getValue(); - inputs.feederAppliedVolts = voltage.getValue(); - inputs.feederCurrentAmps = current.getValue(); - inputs.feederTempC = temp.getValue(); + inputs.feeder = logger.update(); inputs.firstBeambreak = firstBeambreak.get(); inputs.lastBeambreak = lastBeambreak.get(); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java index 590c605d..897f1ae1 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.logging.TalonFXFaultManager; import org.littletonrobotics.junction.Logger; /** Feeder motor for shooter and associated beambreaks for indexing */ @@ -25,6 +26,7 @@ public FeederSubsystem(FeederIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Feeder", inputs); + TalonFXFaultManager.getInstance().addLog("Feeder", inputs.feeder); } /** Run the feeder at a set voltage */ diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index ea4e01ad..86f027c0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -4,20 +4,15 @@ package frc.robot.subsystems.intake; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { @AutoLog public static class IntakeIOInputs { - public double intakeVelocityRotationsPerSecond = 0.0; - public double intakeAppliedVolts = 0.0; - public double intakeCurrentAmps = 0.0; - public double intakeTemperatureCelsius = 0.0; - - public double centeringVelocityRotationsPerSecond = 0.0; - public double centeringAppliedVolts = 0.0; - public double centeringCurrentAmps = 0.0; - public double centeringTemperatureCelsius = 0.0; + public TalonFXLog intake = new TalonFXLog(0, 0, 0, 0, 0, 0); + + public TalonFXLog centering = new TalonFXLog(0, 0, 0, 0, 0, 0); } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 34a18807..ac01c987 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -4,13 +4,12 @@ package frc.robot.subsystems.intake; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import frc.robot.utils.logging.TalonFXLogger; /** Intake IO implementation for TalonFX motors. */ public class IntakeIOReal implements IntakeIO { @@ -24,15 +23,9 @@ public class IntakeIOReal implements IntakeIO { private final VelocityVoltage centeringVelocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); - private final StatusSignal intakeVelocity = intakeMotor.getVelocity(); - private final StatusSignal intakeVoltage = intakeMotor.getMotorVoltage(); - private final StatusSignal intakeAmperage = intakeMotor.getStatorCurrent(); - private final StatusSignal intakeTemp = intakeMotor.getDeviceTemp(); + private final TalonFXLogger intakeLogger = new TalonFXLogger(intakeMotor); - private final StatusSignal centeringVelocity = centeringMotor.getVelocity(); - private final StatusSignal centeringVoltage = centeringMotor.getMotorVoltage(); - private final StatusSignal centeringAmperage = centeringMotor.getStatorCurrent(); - private final StatusSignal centeringTemp = centeringMotor.getDeviceTemp(); + private final TalonFXLogger centeringLogger = new TalonFXLogger(centeringMotor); public IntakeIOReal() { var intakeConfig = new TalonFXConfiguration(); @@ -58,42 +51,13 @@ public IntakeIOReal() { centeringConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.05; centeringMotor.getConfigurator().apply(centeringConfig); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - intakeVelocity, - intakeVoltage, - intakeAmperage, - intakeTemp, - centeringVelocity, - centeringVoltage, - centeringAmperage, - centeringTemp); - intakeMotor.optimizeBusUtilization(); - centeringMotor.optimizeBusUtilization(); } /** Updates the set of loggable inputs. */ @Override public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll( - intakeVelocity, - intakeVoltage, - intakeAmperage, - intakeTemp, - centeringVelocity, - centeringVoltage, - centeringAmperage, - centeringTemp); - inputs.intakeVelocityRotationsPerSecond = intakeVelocity.getValueAsDouble(); - inputs.intakeAppliedVolts = intakeVoltage.getValueAsDouble(); - inputs.intakeCurrentAmps = intakeAmperage.getValueAsDouble(); - inputs.intakeTemperatureCelsius = intakeTemp.getValueAsDouble(); - - inputs.centeringVelocityRotationsPerSecond = centeringVelocity.getValueAsDouble(); - inputs.centeringAppliedVolts = centeringVoltage.getValueAsDouble(); - inputs.centeringCurrentAmps = centeringAmperage.getValueAsDouble(); - inputs.centeringTemperatureCelsius = centeringTemp.getValueAsDouble(); + inputs.intake = intakeLogger.update(); + inputs.centering = centeringLogger.update(); } /** Run the intake at a specified voltage */ diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 8c90b0f2..32e07bf3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.logging.TalonFXFaultManager; import org.littletonrobotics.junction.Logger; /** 95 style utb intake */ @@ -22,6 +23,8 @@ public IntakeSubsystem(IntakeIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); + TalonFXFaultManager.getInstance().addLog("Intake Intake", inputs.intake); + TalonFXFaultManager.getInstance().addLog("Intake Centering", inputs.centering); } /** Run the intake and centering motors at the specified voltage */ diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java index 00df533e..0d8292cd 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java @@ -88,7 +88,8 @@ public Command setRunAlongCmd( }); } - public Command defaultStateDisplayCmd(BooleanSupplier enabled, BooleanSupplier targetIsSpeaker) { + public Command defaultStateDisplayCmd( + BooleanSupplier enabled, BooleanSupplier targetIsSpeaker, BooleanSupplier faultsAreOk) { return Commands.either( Commands.either( this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0) @@ -107,7 +108,7 @@ public Command defaultStateDisplayCmd(BooleanSupplier enabled, BooleanSupplier t return new Color("#0000ff"); } }, - () -> new Color("#350868"), + () -> faultsAreOk.getAsBoolean() ? new Color("#350868") : new Color("#ff5555"), 10, 1.0) .until(enabled), diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 3c4c0cab..e24b9e48 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -5,26 +5,17 @@ package frc.robot.subsystems.shooter; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @AutoLog public static class ShooterIOInputs { - public Rotation2d pivotRotation = new Rotation2d(); - public double pivotVelocityRotationsPerSecond = 0.0; - public double pivotVoltage = 0.0; - public double pivotAmps = 0.0; - public double pivotTempC = 0.0; - - public double flywheelLeftVoltage = 0.0; - public double flywheelLeftVelocityRotationsPerSecond = 0.0; - public double flywheelLeftAmps = 0.0; - public double flywheelLeftTempC = 0.0; - - public double flywheelRightVoltage = 0.0; - public double flywheelRightVelocityRotationsPerSecond = 0.0; - public double flywheelRightAmps = 0.0; - public double flywheelRightTempC = 0.0; + public TalonFXLog pivot = new TalonFXLog(0, 0, 0, 0, 0, 0); + + public TalonFXLog leftFlywheel = new TalonFXLog(0, 0, 0, 0, 0, 0); + + public TalonFXLog rightFlywheel = new TalonFXLog(0, 0, 0, 0, 0, 0); } public void updateInputs(final ShooterIOInputsAutoLogged inputs); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 9c3ba452..33fe218c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.shooter; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -10,27 +8,18 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.utils.logging.TalonFXLogger; public class ShooterIOReal implements ShooterIO { private final TalonFX pivotMotor = new TalonFX(10, "canivore"); private final TalonFX flywheelLeftMotor = new TalonFX(11, "canivore"); private final TalonFX flywheelRightMotor = new TalonFX(12, "canivore"); - private final StatusSignal pivotVelocity = pivotMotor.getVelocity(); - private final StatusSignal pivotVoltage = pivotMotor.getMotorVoltage(); - private final StatusSignal pivotAmps = pivotMotor.getStatorCurrent(); - private final StatusSignal pivotTempC = pivotMotor.getDeviceTemp(); - private final StatusSignal pivotRotations = pivotMotor.getPosition(); + private final TalonFXLogger pivotLogger = new TalonFXLogger(pivotMotor); - private final StatusSignal flywheelLeftAmps = flywheelLeftMotor.getStatorCurrent(); - private final StatusSignal flywheelLeftVoltage = flywheelLeftMotor.getMotorVoltage(); - private final StatusSignal flywheelLeftTempC = flywheelLeftMotor.getDeviceTemp(); - private final StatusSignal flywheelLeftVelocity = flywheelLeftMotor.getVelocity(); + private final TalonFXLogger flywheelLeftLogger = new TalonFXLogger(flywheelLeftMotor); - private final StatusSignal flywheelRightAmps = flywheelRightMotor.getStatorCurrent(); - private final StatusSignal flywheelRightVoltage = flywheelRightMotor.getMotorVoltage(); - private final StatusSignal flywheelRightTempC = flywheelRightMotor.getDeviceTemp(); - private final StatusSignal flywheelRightVelocity = flywheelRightMotor.getVelocity(); + private final TalonFXLogger flywheelRightLogger = new TalonFXLogger(flywheelRightMotor); private final VoltageOut pivotVoltageOut = new VoltageOut(0.0).withEnableFOC(true); private final VoltageOut flywheelLeftVoltageOut = new VoltageOut(0.0).withEnableFOC(true); @@ -67,8 +56,6 @@ public ShooterIOReal() { pivotMotor.getConfigurator().apply(pivotConfig); pivotMotor.setPosition( ShooterSubsystem.PIVOT_MIN_ANGLE.getRotations()); // Assume we boot at hard stop - BaseStatusSignal.setUpdateFrequencyForAll( - 250.0, pivotVelocity, pivotVoltage, pivotAmps, pivotTempC, pivotRotations); pivotMotor.optimizeBusUtilization(); var flywheelConfig = new TalonFXConfiguration(); @@ -92,53 +79,17 @@ public ShooterIOReal() { flywheelLeftMotor.getConfigurator().apply(flywheelConfig); flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; flywheelRightMotor.getConfigurator().apply(flywheelConfig); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - flywheelLeftVelocity, - flywheelLeftVoltage, - flywheelLeftAmps, - flywheelLeftTempC, - flywheelRightVelocity, - flywheelRightVoltage, - flywheelRightAmps, - flywheelRightTempC); flywheelLeftMotor.optimizeBusUtilization(); flywheelRightMotor.optimizeBusUtilization(); } @Override public void updateInputs(ShooterIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll( - pivotRotations, - pivotVelocity, - pivotVoltage, - pivotAmps, - pivotTempC, - flywheelLeftVelocity, - flywheelLeftVoltage, - flywheelLeftAmps, - flywheelLeftTempC, - flywheelRightVelocity, - flywheelRightVoltage, - flywheelRightAmps, - flywheelRightTempC); - - inputs.pivotRotation = Rotation2d.fromRotations(pivotRotations.getValue()); - inputs.pivotVelocityRotationsPerSecond = pivotVelocity.getValue(); - inputs.pivotVoltage = pivotVoltage.getValue(); - inputs.pivotAmps = pivotAmps.getValue(); - inputs.pivotTempC = pivotTempC.getValue(); - - inputs.flywheelLeftVelocityRotationsPerSecond = flywheelLeftVelocity.getValue(); - inputs.flywheelLeftVoltage = flywheelLeftVoltage.getValue(); - inputs.flywheelLeftAmps = flywheelLeftAmps.getValue(); - inputs.flywheelLeftTempC = flywheelLeftTempC.getValue(); - - inputs.flywheelRightVelocityRotationsPerSecond = flywheelRightVelocity.getValue(); - inputs.flywheelRightVoltage = flywheelRightVoltage.getValue(); - inputs.flywheelRightAmps = flywheelRightAmps.getValue(); - inputs.flywheelRightTempC = flywheelRightTempC.getValue(); + inputs.pivot = pivotLogger.update(); + + inputs.leftFlywheel = flywheelLeftLogger.update(); + + inputs.rightFlywheel = flywheelRightLogger.update(); } public void setPivotVoltage(final double voltage) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index dbdc5793..48dce3df 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; /** Add your docs here. */ public class ShooterIOSim implements ShooterIO { @@ -48,23 +49,32 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) { rightFlywheelSim.update(0.020); pivotSim.update(0.020); - inputs.pivotRotation = Rotation2d.fromRadians(pivotSim.getAngleRads()); - inputs.pivotVelocityRotationsPerSecond = - Units.rotationsToRadians(pivotSim.getVelocityRadPerSec()); - inputs.pivotVoltage = 0.0; - inputs.pivotAmps = pivotSim.getCurrentDrawAmps(); - inputs.pivotTempC = 0.0; + inputs.pivot = + new TalonFXLog( + 0.0, + pivotSim.getCurrentDrawAmps(), + pivotSim.getCurrentDrawAmps(), + 0.0, + pivotSim.getAngleRads() / (2 * Math.PI), + pivotSim.getVelocityRadPerSec() / (2 * Math.PI)); - inputs.flywheelLeftVelocityRotationsPerSecond = leftFlywheelSim.getAngularVelocityRPM() / 60.0; - inputs.flywheelLeftVoltage = 0.0; - inputs.flywheelLeftAmps = leftFlywheelSim.getCurrentDrawAmps(); - inputs.flywheelLeftTempC = 0.0; + inputs.leftFlywheel = + new TalonFXLog( + 0.0, + leftFlywheelSim.getCurrentDrawAmps(), + leftFlywheelSim.getCurrentDrawAmps(), + 0.0, + leftFlywheelSim.getAngularPositionRotations(), + leftFlywheelSim.getAngularVelocityRPM() / 60.0); - inputs.flywheelRightVelocityRotationsPerSecond = - rightFlywheelSim.getAngularVelocityRPM() / 60.0; - inputs.flywheelRightVoltage = 0.0; - inputs.flywheelRightAmps = rightFlywheelSim.getCurrentDrawAmps(); - inputs.flywheelRightTempC = 0.0; + inputs.rightFlywheel = + new TalonFXLog( + 0.0, + rightFlywheelSim.getCurrentDrawAmps(), + rightFlywheelSim.getCurrentDrawAmps(), + 0.0, + rightFlywheelSim.getAngularPositionRotations(), + rightFlywheelSim.getAngularVelocityRPM() / 60.0); } public void setPivotVoltage(final double voltage) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 1017c546..ea49a5ed 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.utils.logging.TalonFXFaultManager; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -79,19 +80,28 @@ public ShooterSubsystem(ShooterIO shooterIO) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); - - shooterLig.setAngle(inputs.pivotRotation.unaryMinus().minus(Rotation2d.fromDegrees(180.0))); + TalonFXFaultManager.getInstance().addLog("Shooter Pivot", inputs.pivot); + TalonFXFaultManager.getInstance().addLog("Shooter Left Flywheel", inputs.leftFlywheel); + TalonFXFaultManager.getInstance().addLog("Shooter Right Flywheel", inputs.rightFlywheel); + + shooterLig.setAngle( + Rotation2d.fromRotations(inputs.pivot.position) + .unaryMinus() + .minus(Rotation2d.fromDegrees(180.0))); Logger.recordOutput("Shooter/Mechanism2d", mech2d); Logger.recordOutput("Shooter/Root Pose", getMechanismPose()); } public Pose3d getMechanismPose() { return new Pose3d( - 0.0437896, 0.0, 0.3274568, new Rotation3d(0.0, inputs.pivotRotation.getRadians(), 0.0)); + 0.0437896, + 0.0, + 0.3274568, + new Rotation3d(0.0, Rotation2d.fromRotations(inputs.pivot.position).getRadians(), 0.0)); } public Rotation2d getAngle() { - return inputs.pivotRotation; + return Rotation2d.fromRotations(inputs.pivot.position); } public Command runStateCmd( @@ -105,20 +115,17 @@ public Command runStateCmd( Logger.recordOutput("Shooter/Right Velocity Setpoint", right.getAsDouble()); Logger.recordOutput( "Shooter/Left At Target", - MathUtil.isNear( - left.getAsDouble(), inputs.flywheelLeftVelocityRotationsPerSecond, 1.0)); + MathUtil.isNear(left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Right At Target", - MathUtil.isNear( - right.getAsDouble(), inputs.flywheelRightVelocityRotationsPerSecond, 1.0)); + MathUtil.isNear(right.getAsDouble(), inputs.rightFlywheel.velocity, 1.0)); Logger.recordOutput("Shooter/Rotation Setpoint", rotation.get().getRadians()); Logger.recordOutput( "Shooter/Pivot Error Degrees", - inputs.pivotRotation.getDegrees() - rotation.get().getDegrees()); + (inputs.pivot.position * 360.0) - rotation.get().getDegrees()); Logger.recordOutput( "Shooter/Pivot At Target", - MathUtil.isNear( - rotation.get().getDegrees(), inputs.pivotRotation.getDegrees(), 0.25)); + MathUtil.isNear(rotation.get().getDegrees(), (inputs.pivot.position * 360.0), 0.25)); io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble()); io.setPivotSetpoint(rotation.get()); }); @@ -134,17 +141,15 @@ public Command runFlywheelsCmd(DoubleSupplier left, DoubleSupplier right) { Logger.recordOutput("Shooter/Rotation Setpoint", 0.0); Logger.recordOutput( "Shooter/Left At Target", - MathUtil.isNear( - left.getAsDouble(), inputs.flywheelLeftVelocityRotationsPerSecond, 1.0)); + MathUtil.isNear(left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Right At Target", - MathUtil.isNear( - right.getAsDouble(), inputs.flywheelRightVelocityRotationsPerSecond, 1.0)); + MathUtil.isNear(right.getAsDouble(), inputs.rightFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Pivot At Target", MathUtil.isNear( - right.getAsDouble(), - inputs.flywheelRightVelocityRotationsPerSecond, + PIVOT_MIN_ANGLE.getRotations(), + inputs.pivot.position, Units.degreesToRotations(0.5))); io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble()); io.setPivotVoltage(0.0); @@ -165,7 +170,7 @@ public Command runFlywheelVoltageCmd(Rotation2d rotation, double voltage) { public Command runPivotCurrentZeroing() { return this.run(() -> io.setPivotVoltage(-1.0)) - .until(() -> inputs.pivotAmps > 40.0) + .until(() -> inputs.pivot.statorCurrentAmps > 40.0) .finallyDo(() -> io.resetPivotPosition(PIVOT_MIN_ANGLE)); } @@ -186,43 +191,15 @@ public Command runFlywheelSysidCmd() { this.runOnce(() -> SignalLogger.stop())); } - public Command runPivotSysidCmd() { - return Commands.sequence( - this.runOnce(() -> SignalLogger.start()), - // Stop when we get close to vertical so it falls back - pivotRoutine - .quasistatic(Direction.kForward) - .until(() -> inputs.pivotRotation.getDegrees() > 80.0), - this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)), - Commands.waitSeconds(0.25), - // Stop when near horizontal so we avoid hard stop - pivotRoutine - .quasistatic(Direction.kReverse) - .until(() -> inputs.pivotRotation.getDegrees() < 10.0), - this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)), - Commands.waitSeconds(0.25), - // Stop when we get close to vertical so it falls back - pivotRoutine - .dynamic(Direction.kForward) - .until(() -> inputs.pivotRotation.getDegrees() > 80.0), - this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)), - Commands.waitSeconds(0.25), - // Stop when near horizontal so we avoid hard stop - pivotRoutine - .dynamic(Direction.kReverse) - .until(() -> inputs.pivotRotation.getDegrees() < 10.0), - this.runOnce(() -> SignalLogger.stop())); - } - public Command resetPivotPosition(Rotation2d rotation) { return this.runOnce(() -> io.resetPivotPosition(rotation)); } @AutoLogOutput(key = "Shooter/Is At Goal") public boolean isAtGoal() { - return MathUtil.isNear(rotationGoal.getDegrees(), inputs.pivotRotation.getDegrees(), 0.5) - && MathUtil.isNear(leftGoal, inputs.flywheelLeftVelocityRotationsPerSecond, 1.0) - && MathUtil.isNear(rightGoal, inputs.flywheelRightVelocityRotationsPerSecond, 1.0) - && MathUtil.isNear(0.0, inputs.pivotVelocityRotationsPerSecond, 0.05); + return MathUtil.isNear(rotationGoal.getDegrees(), inputs.pivot.position * 360.0, 0.5) + && MathUtil.isNear(leftGoal, inputs.leftFlywheel.velocity, 1.0) + && MathUtil.isNear(rightGoal, inputs.rightFlywheel.velocity, 1.0) + && MathUtil.isNear(0.0, inputs.pivot.velocity, 0.05); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 375b2333..97019966 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -13,12 +13,12 @@ package frc.robot.subsystems.swerve; -import edu.wpi.first.hal.simulation.RoboRioDataJNI; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXFaultManager; import java.util.List; import org.littletonrobotics.junction.Logger; @@ -64,9 +64,10 @@ public void updateInputs(final List asyncOdometrySamples) { public void periodic() { Logger.processInputs(String.format("Swerve/%s Module", io.getModuleName()), inputs); - Logger.recordOutput( - String.format("Swerve/%s Module/Voltage Available", io.getModuleName()), - Math.abs(inputs.driveAppliedVolts - RoboRioDataJNI.getVInVoltage())); + TalonFXFaultManager.getInstance() + .addLog(String.format("Swerve/%s Module Drive", io.getModuleName()), inputs.drive); + TalonFXFaultManager.getInstance() + .addLog(String.format("Swerve/%s Module Turn", io.getModuleName()), inputs.turn); // Calculate positions for odometry int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together @@ -87,7 +88,11 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) { io.setTurnSetpoint(optimizedState.angle); io.setDriveSetpoint( optimizedState.speedMetersPerSecond - * Math.cos(optimizedState.angle.minus(inputs.turnPosition).getRadians()), + * Math.cos( + optimizedState + .angle + .minus(Rotation2d.fromRotations(inputs.turn.position)) + .getRadians()), (optimizedState.speedMetersPerSecond - lastSetpoint.speedMetersPerSecond) / 0.020); lastSetpoint = optimizedState; @@ -105,7 +110,11 @@ public SwerveModuleState runVoltageSetpoint(SwerveModuleState state) { io.setTurnSetpoint(optimizedState.angle); io.setDriveVoltage( optimizedState.speedMetersPerSecond - * Math.cos(optimizedState.angle.minus(inputs.turnPosition).getRadians())); + * Math.cos( + optimizedState + .angle + .minus(Rotation2d.fromRotations(inputs.turn.position)) + .getRadians())); return optimizedState; } @@ -133,17 +142,17 @@ public void stop() { /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { - return inputs.turnPosition; + return Rotation2d.fromRotations(inputs.turn.position); } /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return inputs.drivePositionMeters; + return inputs.drive.position; } /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return inputs.driveVelocityMetersPerSec; + return inputs.drive.velocity; } /** Returns the module position (turn angle and drive position). */ @@ -163,7 +172,7 @@ public SwerveModulePosition[] getPositionDeltas() { /** Returns the drive velocity in meters/sec. */ public double getCharacterizationVelocity() { - return inputs.driveVelocityMetersPerSec; + return inputs.drive.velocity; } /** Returns the timestamps of the samples received this cycle. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index ead1855c..132a82e9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -15,22 +15,17 @@ import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import java.util.List; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @AutoLog public static class ModuleIOInputs { - public double drivePositionMeters = 0.0; - public double driveVelocityMetersPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; + public TalonFXLog drive = new TalonFXLog(0, 0, 0, 0, 0, 0); public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; + public TalonFXLog turn = new TalonFXLog(0, 0, 0, 0, 0, 0); public double[] odometryDrivePositionsMeters = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index 9917d724..b664c500 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -29,10 +29,10 @@ import com.google.common.collect.ImmutableSet; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.Module.ModuleConstants; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Registration; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXLogger; import java.util.List; /** @@ -59,16 +59,16 @@ public class ModuleIOReal implements ModuleIO { private final CANcoder cancoder; // Signals - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; + private final TalonFXLogger driveLogger; private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; + private final TalonFXLogger turnLogger; + + // Keep separate for async odometry + StatusSignal drivePosition; + StatusSignal turnPosition; + + private double lastUpdate = 0; // Control modes private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); @@ -156,16 +156,13 @@ public ModuleIOReal(ModuleConstants constants) { : SensorDirectionValue.Clockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); - drivePosition = driveTalon.getPosition(); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); + driveLogger = new TalonFXLogger(driveTalon); turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnLogger = new TalonFXLogger(turnTalon); + + drivePosition = driveTalon.getPosition(); turnPosition = turnTalon.getPosition(); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); PhoenixOdometryThread.getInstance() .registerSignals( @@ -174,43 +171,20 @@ public ModuleIOReal(ModuleConstants constants) { BaseStatusSignal.setUpdateFrequencyForAll( Module.ODOMETRY_FREQUENCY_HZ, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnAbsolutePosition); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); } @Override - public void updateInputs(final ModuleIOInputs inputs, final List asyncOdometrySamples) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionMeters = drivePosition.getValueAsDouble(); - inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + public void updateInputs(ModuleIOInputs inputs, final List asyncOdometrySamples) { + BaseStatusSignal.refreshAll(turnAbsolutePosition); + + inputs.drive = driveLogger.update(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + inputs.turn = turnLogger.update(); inputs.odometryTimestamps = asyncOdometrySamples.stream().mapToDouble(s -> s.timestamp()).toArray(); @@ -240,7 +214,7 @@ public void setDriveSetpoint(final double metersPerSecond, final double metersPe // Doesnt actually refresh drive velocity signal, but should be cached if (metersPerSecond == 0 && metersPerSecondSquared == 0 - && MathUtil.isNear(0.0, driveVelocity.getValueAsDouble(), 0.1)) { + && MathUtil.isNear(0.0, driveLogger.log.velocity, 0.1)) { setDriveVoltage(0.0); } else { driveTalon.setControl( diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java index 3a25c112..5751f9cb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import java.util.List; /** @@ -61,21 +62,30 @@ public void updateInputs(final ModuleIOInputs inputs, final List asyncO driveSim.update(LOOP_PERIOD_SECS); turnSim.update(LOOP_PERIOD_SECS); - inputs.drivePositionMeters = driveSim.getAngularPositionRad() * Module.WHEEL_RADIUS; - inputs.driveVelocityMetersPerSec = driveSim.getAngularVelocityRadPerSec() * Module.WHEEL_RADIUS; - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.drive = + new TalonFXLog( + driveAppliedVolts, + driveSim.getCurrentDrawAmps(), + driveSim.getCurrentDrawAmps(), + 0.0, + driveSim.getAngularPositionRad() * Module.WHEEL_RADIUS, + driveSim.getAngularVelocityRadPerSec() * Module.WHEEL_RADIUS); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turn = + new TalonFXLog( + turnAppliedVolts, + turnSim.getCurrentDrawAmps(), + turnSim.getCurrentDrawAmps(), + 0.0, + turnSim.getAngularPositionRad() / (2 * Math.PI), + turnSim.getAngularVelocityRadPerSec() / (2 * Math.PI)); inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsMeters = new double[] {inputs.drivePositionMeters}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + inputs.odometryDrivePositionsMeters = new double[] {inputs.drive.position}; + inputs.odometryTurnPositions = + new Rotation2d[] {Rotation2d.fromRotations(inputs.turn.position)}; } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java index ccaf5463..75b519bc 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java @@ -24,7 +24,7 @@ public class VisionIOReal implements VisionIO { private final VisionConstants constants; /*** Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the camera) in the Robot * Coordinate System. ***/ diff --git a/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java b/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java new file mode 100644 index 00000000..3b845b61 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils.logging; + +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import org.littletonrobotics.junction.Logger; + +/** Class to periodically check all talonfx logs for errors. */ +public class TalonFXFaultManager { + private static TalonFXFaultManager instance; + + private Map logs = new HashMap(20); + private boolean ok = true; + private boolean stickyOk = true; + + private TalonFXFaultManager() {} + + public static TalonFXFaultManager getInstance() { + if (instance == null) instance = new TalonFXFaultManager(); + return instance; + } + + public void addLog(String name, TalonFXLog log) { + logs.put(name, log); + } + + public void periodic() { + ok = true; + List faults = new ArrayList<>(); + for (var log : logs.entrySet()) { + // Check license faults + if (log.getValue().licenseFault) { + ok = false; + faults.add(log.getKey() + " License Fault"); + } + } + stickyOk |= ok; + Logger.recordOutput("Faults/TalonFX Fault Status", ok); + Logger.recordOutput("Faults/TalonFX Sticky Fault Status", stickyOk); + Logger.recordOutput("Faults/TalonFX Faults", faults.stream().reduce("", (a, s) -> a + s)); + logs.clear(); + } + + /** Returns if a fault has happened this loop. */ + public boolean isOk() { + return ok; + } + + /** Returns if a fault has happened since boot. */ + public boolean isOkSticky() { + return stickyOk; + } +} diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java new file mode 100644 index 00000000..0f418e6f --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -0,0 +1,202 @@ +package frc.robot.utils.logging; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import java.nio.ByteBuffer; + +/** + * Helper class for logging TalonFX data with advantagescope. This logs talonFX data using either + * rotations or meters, depending on how the mechanism is configured. + */ +public class TalonFXLogger { + // Use generics for units + public static class TalonFXLog implements StructSerializable { + public double appliedVolts = 0.0; + public double statorCurrentAmps = 0.0; + public double supplyCurrentAmps = 0.0; + public double temperatureCelsius = 0.0; + public double position = 0.0; + public double velocity = 0.0; + + // StatusCode error value. Currently just checks one signal. + public StatusCode errorCode = StatusCode.StatusCodeNotInitialized; + + // Faults + public boolean licenseFault = false; + + public TalonFXLog( + double appliedVolts, + double statorCurrentAmps, + double supplyCurrentAmps, + double temperatureCelsius, + double position, + double velocityRotationsPerSecond, + StatusCode errorCode, + boolean licenseFault) { + this.appliedVolts = appliedVolts; + this.statorCurrentAmps = statorCurrentAmps; + this.supplyCurrentAmps = supplyCurrentAmps; + this.temperatureCelsius = temperatureCelsius; + this.position = position; + this.velocity = velocityRotationsPerSecond; + + this.errorCode = errorCode; + + this.licenseFault = licenseFault; + } + + public TalonFXLog( + double appliedVolts, + double statorCurrentAmps, + double supplyCurrentAmps, + double temperatureCelsius, + double position, + double velocityRotationsPerSecond, + StatusCode errorCode) { + this( + appliedVolts, + statorCurrentAmps, + supplyCurrentAmps, + temperatureCelsius, + position, + velocityRotationsPerSecond, + errorCode, + false); + } + + public TalonFXLog( + double appliedVolts, + double statorCurrentAmps, + double supplyCurrentAmps, + double temperatureCelsius, + double position, + double velocityRotationsPerSecond) { + this( + appliedVolts, + statorCurrentAmps, + supplyCurrentAmps, + temperatureCelsius, + position, + velocityRotationsPerSecond, + StatusCode.StatusCodeNotInitialized, + false); + } + + public static TalonFXLogStruct struct = new TalonFXLogStruct(); + + private static class TalonFXLogStruct implements Struct { + + @Override + public Class getTypeClass() { + return TalonFXLog.class; + } + + @Override + public String getTypeString() { + return "struct:TalonFXLog"; + } + + @Override + public int getSize() { + return kSizeDouble * 6 + kSizeInt32 + kSizeBool * 1; + } + + @Override + public String getSchema() { + return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity;int errorCode;boolean licenseFault"; + } + + @Override + public TalonFXLog unpack(ByteBuffer bb) { + double voltage = bb.getDouble(); + double statorAmps = bb.getDouble(); + double supplyAmps = bb.getDouble(); + double temp = bb.getDouble(); + double rotation = bb.getDouble(); + double velocity = bb.getDouble(); + var errorCode = StatusCode.valueOf(bb.getInt()); + boolean licenseFault = bb.get() != 0; + return new TalonFXLog( + voltage, statorAmps, supplyAmps, temp, rotation, velocity, errorCode, licenseFault); + } + + @Override + public void pack(ByteBuffer bb, TalonFXLog value) { + bb.putDouble(value.appliedVolts); + bb.putDouble(value.statorCurrentAmps); + bb.putDouble(value.supplyCurrentAmps); + bb.putDouble(value.temperatureCelsius); + bb.putDouble(value.position); + bb.putDouble(value.velocity); + bb.put(value.licenseFault ? (byte) 0 : (byte) 1); + } + } + } + + public final TalonFXLog log = new TalonFXLog(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + private final StatusSignal voltageSignal; + private final StatusSignal statorCurrentSignal; + private final StatusSignal supplyCurrentSignal; + private final StatusSignal temperatureSignal; + private final StatusSignal positionSignal; + private final StatusSignal velocitySignal; + + private final StatusSignal licenseFaultSignal; + + public TalonFXLogger( + TalonFX talon, + double updateFrequencyHz, + double faultFrequencyHz, + boolean shouldOptimizeBusUsage) { + voltageSignal = talon.getMotorVoltage(); + statorCurrentSignal = talon.getStatorCurrent(); + supplyCurrentSignal = talon.getSupplyCurrent(); + temperatureSignal = talon.getDeviceTemp(); + positionSignal = talon.getPosition(); + velocitySignal = talon.getVelocity(); + + licenseFaultSignal = talon.getFault_UnlicensedFeatureInUse(); + + BaseStatusSignal.setUpdateFrequencyForAll( + updateFrequencyHz, + voltageSignal, + statorCurrentSignal, + supplyCurrentSignal, + temperatureSignal, + positionSignal, + velocitySignal); + BaseStatusSignal.setUpdateFrequencyForAll(faultFrequencyHz, licenseFaultSignal); + if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); + } + + public TalonFXLogger(TalonFX talon) { + this(talon, 50.0, 1.0, true); + } + + public TalonFXLog update() { + BaseStatusSignal.refreshAll( + voltageSignal, + statorCurrentSignal, + supplyCurrentSignal, + temperatureSignal, + positionSignal, + velocitySignal); + log.appliedVolts = voltageSignal.getValueAsDouble(); + log.statorCurrentAmps = statorCurrentSignal.getValueAsDouble(); + log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); + log.temperatureCelsius = temperatureSignal.getValueAsDouble(); + log.position = positionSignal.getValueAsDouble(); + log.velocity = velocitySignal.getValueAsDouble(); + + log.errorCode = voltageSignal.getStatus(); + + log.licenseFault = licenseFaultSignal.getValue(); + + return log; + } +}