From 774423be54df7b1448d14cbbcdfd0aa3f97d32a9 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Mon, 26 Feb 2024 15:08:50 -0800 Subject: [PATCH 01/27] added centering motor statussignals to refreshall call --- .../java/frc/robot/subsystems/intake/IntakeIOReal.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index 297503a4..d486a937 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -55,7 +55,15 @@ public IntakeIOReal() { /** Updates the set of loggable inputs. */ @Override public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll(intakeVelocity, intakeVoltage, intakeAmperage, intakeTemp); + BaseStatusSignal.refreshAll( + intakeVelocity, + intakeVoltage, + intakeAmperage, + intakeTemp, + centeringVelocity, + centeringVoltage, + centeringAmperage, + centeringTemp); inputs.intakeVelocityRotationsPerSecond = intakeVelocity.getValueAsDouble(); inputs.intakeAppliedVolts = intakeVoltage.getValueAsDouble(); inputs.intakeCurrentAmps = intakeAmperage.getValueAsDouble(); From a5901d3df66517666dcc7087c8f0810367f21744 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 13:47:35 -0800 Subject: [PATCH 02/27] added stator current logging to swerve --- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 ++-- .../robot/subsystems/swerve/ModuleIOReal.java | 28 ++++++++++++------- .../robot/subsystems/swerve/ModuleIOSim.java | 4 +-- 3 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 28473040..c17188a6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -22,13 +22,15 @@ 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 double driveStatorCurrentAmps = 0.0; + public double driveSupplyCurrentAmps = 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 double turnStatorCurrentAmps = 0.0; + public double turnSupplyCurrentAmps = 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 a5f7c88e..fbef8462 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -58,14 +58,16 @@ public class ModuleIOReal implements ModuleIO { private final Queue drivePositionQueue; private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; + private final StatusSignal driveStatorCurrent; + private final StatusSignal driveSupplyCurrent; private final StatusSignal turnAbsolutePosition; private final StatusSignal turnPosition; private final Queue turnPositionQueue; private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; + private final StatusSignal turnStatorCurrent; + private final StatusSignal turnSupplyCurrent; // Control modes private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); @@ -148,7 +150,8 @@ public ModuleIOReal(ModuleConstants constants) { PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); + driveStatorCurrent = driveTalon.getStatorCurrent(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); @@ -156,7 +159,8 @@ public ModuleIOReal(ModuleConstants constants) { PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); + turnStatorCurrent = turnTalon.getStatorCurrent(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( Module.ODOMETRY_FREQUENCY_HZ, drivePosition, turnPosition); @@ -164,11 +168,13 @@ public ModuleIOReal(ModuleConstants constants) { 50.0, driveVelocity, driveAppliedVolts, - driveCurrent, + driveStatorCurrent, + driveSupplyCurrent, turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnStatorCurrent, + turnSupplyCurrent); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); @@ -180,23 +186,25 @@ public void updateInputs(ModuleIOInputs inputs) { drivePosition, driveVelocity, driveAppliedVolts, - driveCurrent, + driveStatorCurrent, turnAbsolutePosition, turnPosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnStatorCurrent); inputs.drivePositionMeters = drivePosition.getValueAsDouble(); inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + inputs.driveStatorCurrentAmps = driveStatorCurrent.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); 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.turnStatorCurrentAmps = turnStatorCurrent.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); inputs.odometryDrivePositionsMeters = drivePositionQueue.stream().mapToDouble(Units::rotationsToRadians).toArray(); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java index 6d2f3417..5a09d6ee 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java @@ -61,14 +61,14 @@ public void updateInputs(final ModuleIOInputs inputs) { 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.driveStatorCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); 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.turnStatorCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); inputs.odometryDrivePositionsMeters = new double[] {inputs.drivePositionMeters}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; From a38795ee56d07ebbee5c2a2eebd05dcb9c664977 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 14:21:07 -0800 Subject: [PATCH 03/27] added supply current logging to shooter --- .../robot/subsystems/shooter/ShooterIO.java | 9 +++-- .../subsystems/shooter/ShooterIOReal.java | 36 ++++++++++++------- .../subsystems/shooter/ShooterIOSim.java | 6 ++-- .../subsystems/shooter/ShooterSubystem.java | 2 +- 4 files changed, 33 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 1c03e64d..e65c9a3a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -13,17 +13,20 @@ 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 pivotStatorCurrentAmps = 0.0; + public double pivotSupplyCurrentAmps = 0.0; public double pivotTempC = 0.0; public double flywheelLeftVoltage = 0.0; public double flywheelLeftVelocityRotationsPerSecond = 0.0; - public double flywheelLeftAmps = 0.0; + public double flywheelLeftStatorCurrentAmps = 0.0; + public double flywheelLeftSupplyCurrentAmps = 0.0; public double flywheelLeftTempC = 0.0; public double flywheelRightVoltage = 0.0; public double flywheelRightVelocityRotationsPerSecond = 0.0; - public double flywheelRightAmps = 0.0; + public double flywheelRightStatorCurrentAmps = 0.0; + public double flywheelRightSupplyCurrentAmps = 0.0; public double flywheelRightTempC = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 4ed9283f..7a60cd70 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -18,16 +18,19 @@ public class ShooterIOReal implements ShooterIO { private final StatusSignal pivotVelocity = pivotMotor.getVelocity(); private final StatusSignal pivotVoltage = pivotMotor.getMotorVoltage(); - private final StatusSignal pivotAmps = pivotMotor.getStatorCurrent(); + private final StatusSignal pivotStatorCurrentAmps = pivotMotor.getStatorCurrent(); + private final StatusSignal pivotSupplyCurrentAmps = pivotMotor.getSupplyCurrent(); private final StatusSignal pivotTempC = pivotMotor.getDeviceTemp(); private final StatusSignal pivotRotations = pivotMotor.getPosition(); - private final StatusSignal flywheelLeftAmps = flywheelLeftMotor.getStatorCurrent(); + private final StatusSignal flywheelLeftStatorCurrentAmps = flywheelLeftMotor.getStatorCurrent(); + private final StatusSignal flywheelLeftSupplyCurrentAmps = flywheelLeftMotor.getSupplyCurrent(); private final StatusSignal flywheelLeftVoltage = flywheelLeftMotor.getMotorVoltage(); private final StatusSignal flywheelLeftTempC = flywheelLeftMotor.getDeviceTemp(); private final StatusSignal flywheelLeftVelocity = flywheelLeftMotor.getVelocity(); - private final StatusSignal flywheelRightAmps = flywheelRightMotor.getStatorCurrent(); + private final StatusSignal flywheelRightStatorCurrentAmps = flywheelRightMotor.getStatorCurrent(); + private final StatusSignal flywheelRightSupplyCurrentAmps = flywheelRightMotor.getSupplyCurrent(); private final StatusSignal flywheelRightVoltage = flywheelRightMotor.getMotorVoltage(); private final StatusSignal flywheelRightTempC = flywheelRightMotor.getDeviceTemp(); private final StatusSignal flywheelRightVelocity = flywheelRightMotor.getVelocity(); @@ -68,13 +71,12 @@ public ShooterIOReal() { pivotMotor.setPosition( ShooterSubystem.PIVOT_MIN_ANGLE.getRotations()); // Assume we boot at hard stop BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, pivotVelocity, pivotVoltage, pivotAmps, pivotTempC, pivotRotations); + 50.0, pivotVelocity, pivotVoltage, pivotStatorCurrentAmps, pivotSupplyCurrentAmps, pivotTempC, pivotRotations); pivotMotor.optimizeBusUtilization(); var flywheelConfig = new TalonFXConfiguration(); flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - flywheelConfig.Feedback.SensorToMechanismRatio = ShooterSubystem.FLYWHEEL_RATIO; // TODO add in once cad is done @@ -96,11 +98,13 @@ public ShooterIOReal() { 50.0, flywheelLeftVelocity, flywheelLeftVoltage, - flywheelLeftAmps, + flywheelLeftStatorCurrentAmps, + flywheelLeftSupplyCurrentAmps, flywheelLeftTempC, flywheelRightVelocity, flywheelRightVoltage, - flywheelRightAmps, + flywheelRightStatorCurrentAmps, + flywheelRightSupplyCurrentAmps, flywheelRightTempC); flywheelLeftMotor.optimizeBusUtilization(); flywheelRightMotor.optimizeBusUtilization(); @@ -112,31 +116,37 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) { pivotRotations, pivotVelocity, pivotVoltage, - pivotAmps, + pivotStatorCurrentAmps, + pivotSupplyCurrentAmps, pivotTempC, flywheelLeftVelocity, flywheelLeftVoltage, - flywheelLeftAmps, + flywheelLeftStatorCurrentAmps, + flywheelLeftSupplyCurrentAmps, flywheelLeftTempC, flywheelRightVelocity, flywheelRightVoltage, - flywheelRightAmps, + flywheelRightStatorCurrentAmps, + flywheelRightSupplyCurrentAmps, flywheelRightTempC); inputs.pivotRotation = Rotation2d.fromRotations(pivotRotations.getValue()); inputs.pivotVelocityRotationsPerSecond = pivotVelocity.getValue(); inputs.pivotVoltage = pivotVoltage.getValue(); - inputs.pivotAmps = pivotAmps.getValue(); + inputs.pivotStatorCurrentAmps = pivotStatorCurrentAmps.getValue(); + inputs.pivotSupplyCurrentAmps = pivotSupplyCurrentAmps.getValue(); inputs.pivotTempC = pivotTempC.getValue(); inputs.flywheelLeftVelocityRotationsPerSecond = flywheelLeftVelocity.getValue(); inputs.flywheelLeftVoltage = flywheelLeftVoltage.getValue(); - inputs.flywheelLeftAmps = flywheelLeftAmps.getValue(); + inputs.flywheelLeftStatorCurrentAmps = flywheelLeftStatorCurrentAmps.getValue(); + inputs.flywheelLeftSupplyCurrentAmps = flywheelLeftSupplyCurrentAmps.getValue(); inputs.flywheelLeftTempC = flywheelLeftTempC.getValue(); inputs.flywheelRightVelocityRotationsPerSecond = flywheelRightVelocity.getValue(); inputs.flywheelRightVoltage = flywheelRightVoltage.getValue(); - inputs.flywheelRightAmps = flywheelRightAmps.getValue(); + inputs.flywheelRightStatorCurrentAmps = flywheelRightStatorCurrentAmps.getValue(); + inputs.flywheelRightSupplyCurrentAmps = flywheelRightSupplyCurrentAmps.getValue(); inputs.flywheelRightTempC = flywheelRightTempC.getValue(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 97d424b5..5b6a88e9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -52,18 +52,18 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) { inputs.pivotVelocityRotationsPerSecond = Units.rotationsToRadians(pivotSim.getVelocityRadPerSec()); inputs.pivotVoltage = 0.0; - inputs.pivotAmps = pivotSim.getCurrentDrawAmps(); + inputs.pivotStatorCurrentAmps = pivotSim.getCurrentDrawAmps(); inputs.pivotTempC = 0.0; inputs.flywheelLeftVelocityRotationsPerSecond = leftFlywheelSim.getAngularVelocityRPM() / 60.0; inputs.flywheelLeftVoltage = 0.0; - inputs.flywheelLeftAmps = leftFlywheelSim.getCurrentDrawAmps(); + inputs.flywheelLeftStatorCurrentAmps = leftFlywheelSim.getCurrentDrawAmps(); inputs.flywheelLeftTempC = 0.0; inputs.flywheelRightVelocityRotationsPerSecond = rightFlywheelSim.getAngularVelocityRPM() / 60.0; inputs.flywheelRightVoltage = 0.0; - inputs.flywheelRightAmps = rightFlywheelSim.getCurrentDrawAmps(); + inputs.flywheelRightStatorCurrentAmps = rightFlywheelSim.getCurrentDrawAmps(); inputs.flywheelRightTempC = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java index 07feb35e..0764c2a3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java @@ -110,7 +110,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.pivotStatorCurrentAmps > 40.0) .finallyDo(() -> io.resetPivotPostion(PIVOT_MIN_ANGLE)); } From 9d848f8088ef633cc173cdeaffd76d9b8dd40e3d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 14:25:35 -0800 Subject: [PATCH 04/27] add supply current to intake --- .../frc/robot/subsystems/intake/IntakeIO.java | 6 +++-- .../robot/subsystems/intake/IntakeIOReal.java | 24 ++++++++++++------- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index a709888c..5038933f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -11,12 +11,14 @@ public interface IntakeIO { public static class IntakeIOInputs { public double intakeVelocityRotationsPerSecond = 0.0; public double intakeAppliedVolts = 0.0; - public double intakeCurrentAmps = 0.0; + public double intakeStatorCurrentAmps = 0.0; + public double intakeSupplyCurrentAmps = 0.0; public double intakeTemperatureCelsius = 0.0; public double centeringVelocityRotationsPerSecond = 0.0; public double centeringAppliedVolts = 0.0; - public double centeringCurrentAmps = 0.0; + public double centeringStatorCurrentAmps = 0.0; + public double centeringSupplyCurrentAmps = 0.0; public double centeringTemperatureCelsius = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java index d486a937..a3e1114e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -21,12 +21,14 @@ public class IntakeIOReal implements IntakeIO { private final StatusSignal intakeVelocity = intakeMotor.getVelocity(); private final StatusSignal intakeVoltage = intakeMotor.getMotorVoltage(); - private final StatusSignal intakeAmperage = intakeMotor.getStatorCurrent(); + private final StatusSignal intakeStatorCurrentAmps = intakeMotor.getStatorCurrent(); + private final StatusSignal intakeSupplyCurrentAmps = intakeMotor.getSupplyCurrent(); private final StatusSignal intakeTemp = intakeMotor.getDeviceTemp(); private final StatusSignal centeringVelocity = centeringMotor.getVelocity(); private final StatusSignal centeringVoltage = centeringMotor.getMotorVoltage(); - private final StatusSignal centeringAmperage = centeringMotor.getStatorCurrent(); + private final StatusSignal centeringStatorCurrentAmps = centeringMotor.getStatorCurrent(); + private final StatusSignal centeringSupplyCurrentAmps = centeringMotor.getSupplyCurrent(); private final StatusSignal centeringTemp = centeringMotor.getDeviceTemp(); public IntakeIOReal() { @@ -42,11 +44,13 @@ public IntakeIOReal() { 50.0, intakeVelocity, intakeVoltage, - intakeAmperage, + intakeStatorCurrentAmps, + intakeSupplyCurrentAmps, intakeTemp, centeringVelocity, centeringVoltage, - centeringAmperage, + centeringStatorCurrentAmps, + centeringSupplyCurrentAmps, centeringTemp); intakeMotor.optimizeBusUtilization(); centeringMotor.optimizeBusUtilization(); @@ -58,20 +62,24 @@ public void updateInputs(IntakeIOInputs inputs) { BaseStatusSignal.refreshAll( intakeVelocity, intakeVoltage, - intakeAmperage, + intakeStatorCurrentAmps, + intakeSupplyCurrentAmps, intakeTemp, centeringVelocity, centeringVoltage, - centeringAmperage, + centeringStatorCurrentAmps, + centeringSupplyCurrentAmps, centeringTemp); inputs.intakeVelocityRotationsPerSecond = intakeVelocity.getValueAsDouble(); inputs.intakeAppliedVolts = intakeVoltage.getValueAsDouble(); - inputs.intakeCurrentAmps = intakeAmperage.getValueAsDouble(); + inputs.intakeStatorCurrentAmps = intakeStatorCurrentAmps.getValueAsDouble(); + inputs.intakeSupplyCurrentAmps = intakeSupplyCurrentAmps.getValueAsDouble(); inputs.intakeTemperatureCelsius = intakeTemp.getValueAsDouble(); inputs.centeringVelocityRotationsPerSecond = centeringVelocity.getValueAsDouble(); inputs.centeringAppliedVolts = centeringVoltage.getValueAsDouble(); - inputs.centeringCurrentAmps = centeringAmperage.getValueAsDouble(); + inputs.centeringStatorCurrentAmps = centeringStatorCurrentAmps.getValueAsDouble(); + inputs.centeringSupplyCurrentAmps = centeringSupplyCurrentAmps.getValueAsDouble(); inputs.centeringTemperatureCelsius = centeringTemp.getValueAsDouble(); } From 1149b9cb9d224cd0bd8f02557784957ff5552b0b Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 14:32:01 -0800 Subject: [PATCH 05/27] added feeder supply current logging --- .../java/frc/robot/subsystems/feeder/FeederIO.java | 3 ++- .../java/frc/robot/subsystems/feeder/FeederIOReal.java | 10 ++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index f64d5ef7..d451be75 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -11,7 +11,8 @@ public interface FeederIO { public static class FeederIOInputs { public double feederVelocityRotationsPerSec = 0.0; public double feederAppliedVolts = 0.0; - public double feederCurrentAmps = 0.0; + public double feederStatorCurrentAmps = 0.0; + public double feederSupplyCurrentAmps = 0.0; public double feederTempC = 0.0; public boolean firstBeambreak = false; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java index c8afed61..c124ef89 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -21,7 +21,8 @@ public class FeederIOReal implements FeederIO { private final StatusSignal velocity = motor.getVelocity(); private final StatusSignal voltage = motor.getMotorVoltage(); - private final StatusSignal current = motor.getStatorCurrent(); + private final StatusSignal statorCurrent = motor.getStatorCurrent(); + private final StatusSignal supplyCurrent = motor.getSupplyCurrent(); private final StatusSignal temp = motor.getDeviceTemp(); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); @@ -36,17 +37,18 @@ public FeederIOReal() { motor.getConfigurator().apply(config); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, current, temp); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, statorCurrent, temp, supplyCurrent); motor.optimizeBusUtilization(); } @Override public void updateInputs(final FeederIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, current, temp); + BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp); inputs.feederVelocityRotationsPerSec = velocity.getValue(); inputs.feederAppliedVolts = voltage.getValue(); - inputs.feederCurrentAmps = current.getValue(); + inputs.feederStatorCurrentAmps = statorCurrent.getValue(); + inputs.feederSupplyCurrentAmps = supplyCurrent.getValue(); inputs.feederTempC = temp.getValue(); inputs.firstBeambreak = firstBeambreak.get(); From fc18614998fd54cb07377e76fe9d9c02af600646 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 14:35:47 -0800 Subject: [PATCH 06/27] added elevator supply and follower logging --- .../robot/subsystems/elevator/ElevatorIO.java | 3 ++- .../subsystems/elevator/ElevatorIOReal.java | 16 +++++++++++----- .../robot/subsystems/elevator/ElevatorIOSim.java | 2 +- .../subsystems/elevator/ElevatorSubsystem.java | 2 +- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index a6835104..905541ff 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -12,7 +12,8 @@ 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[] elevatorStatorCurrentAmps = new double[] {}; + public double[] elevatorSupplyCurrentAmps = new double[] {}; public double[] elevatorTempCelsius = new double[] {}; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 2c87f3de..fd993272 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -26,9 +26,14 @@ public class ElevatorIOReal implements ElevatorIO { 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 statorCurrent = motor.getStatorCurrent(); + private final StatusSignal supplyCurrent = motor.getSupplyCurrent(); private final StatusSignal temp = motor.getDeviceTemp(); + private final StatusSignal followerStatorCurrent = follower.getStatorCurrent(); + private final StatusSignal followerSupplyCurrent = follower.getSupplyCurrent(); + private final StatusSignal followerTemp = follower.getDeviceTemp(); + public ElevatorIOReal() { var config = new TalonFXConfiguration(); @@ -59,19 +64,20 @@ public ElevatorIOReal() { follower.getConfigurator().apply(new TalonFXConfiguration()); follower.setControl(new Follower(motor.getDeviceID(), true)); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, voltage, current, temp); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, voltage, statorCurrent, supplyCurrent, temp, followerStatorCurrent, followerSupplyCurrent, followerTemp); motor.optimizeBusUtilization(); follower.optimizeBusUtilization(); } @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(position, velocity, voltage, current, temp); + BaseStatusSignal.refreshAll(position, velocity, voltage, statorCurrent, supplyCurrent, temp, followerStatorCurrent, followerSupplyCurrent, followerTemp); 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.elevatorStatorCurrentAmps = new double[] {statorCurrent.getValueAsDouble(), followerStatorCurrent.getValueAsDouble()}; + inputs.elevatorSupplyCurrentAmps = new double[] {supplyCurrent.getValueAsDouble(), followerSupplyCurrent.getValueAsDouble()}; + inputs.elevatorTempCelsius = new double[] {temp.getValueAsDouble(), followerTemp.getValueAsDouble()}; } @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 8bda49ab..efe6643d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -39,7 +39,7 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { inputs.elevatorPositionMeters = physicsSim.getPositionMeters(); inputs.elevatorVelocityMetersPerSec = physicsSim.getVelocityMetersPerSecond(); inputs.elevatorAppliedVolts = volts; - inputs.elevatorCurrentAmps = new double[] {physicsSim.getCurrentDrawAmps()}; + inputs.elevatorStatorCurrentAmps = new double[] {physicsSim.getCurrentDrawAmps()}; inputs.elevatorTempCelsius = new double[] {20.0}; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index e24b6678..09b8de95 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -91,7 +91,7 @@ public Command setExtensionCmd(DoubleSupplier meters) { public Command runCurrentZeroing() { return this.run(() -> io.setVoltage(-1.0)) - .until(() -> inputs.elevatorCurrentAmps[0] > 40.0) + .until(() -> inputs.elevatorStatorCurrentAmps[0] > 40.0) .finallyDo(() -> io.resetEncoder(0.0)); } From fc33e99f583f91cbf761745b92a0cc91c47ff5c2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 14:38:46 -0800 Subject: [PATCH 07/27] added carriage supply current logging --- .../frc/robot/subsystems/carriage/CarriageIO.java | 6 +++--- .../robot/subsystems/carriage/CarriageIOReal.java | 12 +++++++----- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java index 42b91cec..ccbe6347 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java @@ -11,9 +11,9 @@ public interface CarriageIO { 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 double statorCurrentAmps = 0.0; + public double supplyCurrentAmps = 0.0; + public double temperatureCelsius = 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 7b73aa1e..72dcb39f 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java @@ -22,7 +22,8 @@ public class CarriageIOReal implements CarriageIO { final StatusSignal velocity = motor.getVelocity(); final StatusSignal voltage = motor.getMotorVoltage(); - final StatusSignal amperage = motor.getStatorCurrent(); + final StatusSignal statorCurrent = motor.getStatorCurrent(); + final StatusSignal supplyCurrent = motor.getSupplyCurrent(); final StatusSignal temp = motor.getDeviceTemp(); public CarriageIOReal() { @@ -31,18 +32,19 @@ public CarriageIOReal() { config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; motor.getConfigurator().apply(config); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, amperage, temp); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, statorCurrent, supplyCurrent, temp); motor.optimizeBusUtilization(); } /** Updates the set of loggable inputs. */ @Override public void updateInputs(final CarriageIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, amperage, temp); + BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp); inputs.velocityRotationsPerSecond = velocity.getValueAsDouble(); inputs.appliedVolts = voltage.getValueAsDouble(); - inputs.currentAmps = new double[] {amperage.getValueAsDouble()}; - inputs.temperatureCelsius = new double[] {temp.getValueAsDouble()}; + inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); + inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); + inputs.temperatureCelsius = temp.getValueAsDouble(); inputs.beambreak = beambreak.get(); } From 53d8a1f692dcc9c8e2a47be848c8d516e209dada Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 14:40:25 -0800 Subject: [PATCH 08/27] standardize on getValueAsDouble --- .../robot/subsystems/feeder/FeederIOReal.java | 10 +++--- .../subsystems/shooter/ShooterIOReal.java | 36 +++++++++---------- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java index c124ef89..604a53c1 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -45,11 +45,11 @@ public FeederIOReal() { public void updateInputs(final FeederIOInputsAutoLogged inputs) { BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp); - inputs.feederVelocityRotationsPerSec = velocity.getValue(); - inputs.feederAppliedVolts = voltage.getValue(); - inputs.feederStatorCurrentAmps = statorCurrent.getValue(); - inputs.feederSupplyCurrentAmps = supplyCurrent.getValue(); - inputs.feederTempC = temp.getValue(); + inputs.feederVelocityRotationsPerSec = velocity.getValueAsDouble(); + inputs.feederAppliedVolts = voltage.getValueAsDouble(); + inputs.feederStatorCurrentAmps = statorCurrent.getValueAsDouble(); + inputs.feederSupplyCurrentAmps = supplyCurrent.getValueAsDouble(); + inputs.feederTempC = temp.getValueAsDouble(); inputs.firstBeambreak = firstBeambreak.get(); inputs.lastBeambreak = lastBeambreak.get(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 7a60cd70..f1a78d18 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -130,24 +130,24 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) { flywheelRightSupplyCurrentAmps, flywheelRightTempC); - inputs.pivotRotation = Rotation2d.fromRotations(pivotRotations.getValue()); - inputs.pivotVelocityRotationsPerSecond = pivotVelocity.getValue(); - inputs.pivotVoltage = pivotVoltage.getValue(); - inputs.pivotStatorCurrentAmps = pivotStatorCurrentAmps.getValue(); - inputs.pivotSupplyCurrentAmps = pivotSupplyCurrentAmps.getValue(); - inputs.pivotTempC = pivotTempC.getValue(); - - inputs.flywheelLeftVelocityRotationsPerSecond = flywheelLeftVelocity.getValue(); - inputs.flywheelLeftVoltage = flywheelLeftVoltage.getValue(); - inputs.flywheelLeftStatorCurrentAmps = flywheelLeftStatorCurrentAmps.getValue(); - inputs.flywheelLeftSupplyCurrentAmps = flywheelLeftSupplyCurrentAmps.getValue(); - inputs.flywheelLeftTempC = flywheelLeftTempC.getValue(); - - inputs.flywheelRightVelocityRotationsPerSecond = flywheelRightVelocity.getValue(); - inputs.flywheelRightVoltage = flywheelRightVoltage.getValue(); - inputs.flywheelRightStatorCurrentAmps = flywheelRightStatorCurrentAmps.getValue(); - inputs.flywheelRightSupplyCurrentAmps = flywheelRightSupplyCurrentAmps.getValue(); - inputs.flywheelRightTempC = flywheelRightTempC.getValue(); + inputs.pivotRotation = Rotation2d.fromRotations(pivotRotations.getValueAsDouble()); + inputs.pivotVelocityRotationsPerSecond = pivotVelocity.getValueAsDouble(); + inputs.pivotVoltage = pivotVoltage.getValueAsDouble(); + inputs.pivotStatorCurrentAmps = pivotStatorCurrentAmps.getValueAsDouble(); + inputs.pivotSupplyCurrentAmps = pivotSupplyCurrentAmps.getValueAsDouble(); + inputs.pivotTempC = pivotTempC.getValueAsDouble(); + + inputs.flywheelLeftVelocityRotationsPerSecond = flywheelLeftVelocity.getValueAsDouble(); + inputs.flywheelLeftVoltage = flywheelLeftVoltage.getValueAsDouble(); + inputs.flywheelLeftStatorCurrentAmps = flywheelLeftStatorCurrentAmps.getValueAsDouble(); + inputs.flywheelLeftSupplyCurrentAmps = flywheelLeftSupplyCurrentAmps.getValueAsDouble(); + inputs.flywheelLeftTempC = flywheelLeftTempC.getValueAsDouble(); + + inputs.flywheelRightVelocityRotationsPerSecond = flywheelRightVelocity.getValueAsDouble(); + inputs.flywheelRightVoltage = flywheelRightVoltage.getValueAsDouble(); + inputs.flywheelRightStatorCurrentAmps = flywheelRightStatorCurrentAmps.getValueAsDouble(); + inputs.flywheelRightSupplyCurrentAmps = flywheelRightSupplyCurrentAmps.getValueAsDouble(); + inputs.flywheelRightTempC = flywheelRightTempC.getValueAsDouble(); } public void setPivotVoltage(final double voltage) { From c894f14a2e3a71ccf9beca07a35b99d373c202d9 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 27 Feb 2024 22:56:00 +0000 Subject: [PATCH 09/27] run spotless --- .../subsystems/carriage/CarriageIOReal.java | 3 +- .../subsystems/elevator/ElevatorIOReal.java | 32 ++++++++++++++++--- .../robot/subsystems/feeder/FeederIOReal.java | 3 +- .../subsystems/shooter/ShooterIOReal.java | 20 +++++++++--- 4 files changed, 46 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java index c2a302c0..e3fc2f6e 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java @@ -32,7 +32,8 @@ public CarriageIOReal() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; motor.getConfigurator().apply(config); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, statorCurrent, supplyCurrent, temp); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, velocity, voltage, statorCurrent, supplyCurrent, temp); motor.optimizeBusUtilization(); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 53f1b7c9..1ccb3bf5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -64,20 +64,42 @@ public ElevatorIOReal() { follower.getConfigurator().apply(new TalonFXConfiguration()); follower.setControl(new Follower(motor.getDeviceID(), true)); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, voltage, statorCurrent, supplyCurrent, temp, followerStatorCurrent, followerSupplyCurrent, followerTemp); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + position, + velocity, + voltage, + statorCurrent, + supplyCurrent, + temp, + followerStatorCurrent, + followerSupplyCurrent, + followerTemp); motor.optimizeBusUtilization(); follower.optimizeBusUtilization(); } @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(position, velocity, voltage, statorCurrent, supplyCurrent, temp, followerStatorCurrent, followerSupplyCurrent, followerTemp); + BaseStatusSignal.refreshAll( + position, + velocity, + voltage, + statorCurrent, + supplyCurrent, + temp, + followerStatorCurrent, + followerSupplyCurrent, + followerTemp); inputs.elevatorPositionMeters = position.getValueAsDouble(); inputs.elevatorVelocityMetersPerSec = velocity.getValueAsDouble(); inputs.elevatorAppliedVolts = voltage.getValueAsDouble(); - inputs.elevatorStatorCurrentAmps = new double[] {statorCurrent.getValueAsDouble(), followerStatorCurrent.getValueAsDouble()}; - inputs.elevatorSupplyCurrentAmps = new double[] {supplyCurrent.getValueAsDouble(), followerSupplyCurrent.getValueAsDouble()}; - inputs.elevatorTempCelsius = new double[] {temp.getValueAsDouble(), followerTemp.getValueAsDouble()}; + inputs.elevatorStatorCurrentAmps = + new double[] {statorCurrent.getValueAsDouble(), followerStatorCurrent.getValueAsDouble()}; + inputs.elevatorSupplyCurrentAmps = + new double[] {supplyCurrent.getValueAsDouble(), followerSupplyCurrent.getValueAsDouble()}; + inputs.elevatorTempCelsius = + new double[] {temp.getValueAsDouble(), followerTemp.getValueAsDouble()}; } @Override diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java index 11e26785..4f685750 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -37,7 +37,8 @@ public FeederIOReal() { motor.getConfigurator().apply(config); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, statorCurrent, temp, supplyCurrent); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, velocity, voltage, statorCurrent, temp, supplyCurrent); motor.optimizeBusUtilization(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 9ddf8bf3..d69aa6e8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -23,14 +23,18 @@ public class ShooterIOReal implements ShooterIO { private final StatusSignal pivotTempC = pivotMotor.getDeviceTemp(); private final StatusSignal pivotRotations = pivotMotor.getPosition(); - private final StatusSignal flywheelLeftStatorCurrentAmps = flywheelLeftMotor.getStatorCurrent(); - private final StatusSignal flywheelLeftSupplyCurrentAmps = flywheelLeftMotor.getSupplyCurrent(); + private final StatusSignal flywheelLeftStatorCurrentAmps = + flywheelLeftMotor.getStatorCurrent(); + private final StatusSignal flywheelLeftSupplyCurrentAmps = + flywheelLeftMotor.getSupplyCurrent(); private final StatusSignal flywheelLeftVoltage = flywheelLeftMotor.getMotorVoltage(); private final StatusSignal flywheelLeftTempC = flywheelLeftMotor.getDeviceTemp(); private final StatusSignal flywheelLeftVelocity = flywheelLeftMotor.getVelocity(); - private final StatusSignal flywheelRightStatorCurrentAmps = flywheelRightMotor.getStatorCurrent(); - private final StatusSignal flywheelRightSupplyCurrentAmps = flywheelRightMotor.getSupplyCurrent(); + private final StatusSignal flywheelRightStatorCurrentAmps = + flywheelRightMotor.getStatorCurrent(); + private final StatusSignal flywheelRightSupplyCurrentAmps = + flywheelRightMotor.getSupplyCurrent(); private final StatusSignal flywheelRightVoltage = flywheelRightMotor.getMotorVoltage(); private final StatusSignal flywheelRightTempC = flywheelRightMotor.getDeviceTemp(); private final StatusSignal flywheelRightVelocity = flywheelRightMotor.getVelocity(); @@ -71,7 +75,13 @@ public ShooterIOReal() { pivotMotor.setPosition( ShooterSubystem.PIVOT_MIN_ANGLE.getRotations()); // Assume we boot at hard stop BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, pivotVelocity, pivotVoltage, pivotStatorCurrentAmps, pivotSupplyCurrentAmps, pivotTempC, pivotRotations); + 50.0, + pivotVelocity, + pivotVoltage, + pivotStatorCurrentAmps, + pivotSupplyCurrentAmps, + pivotTempC, + pivotRotations); pivotMotor.optimizeBusUtilization(); var flywheelConfig = new TalonFXConfiguration(); From c832c9a0bab5406da5f102ded4f46aad0f577a22 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 28 Feb 2024 10:49:00 -0800 Subject: [PATCH 10/27] added TalonFXLogger --- simgui-ds.json | 17 ++- simgui.json | 49 +++++++- .../robot/utils/logging/TalonFXLogger.java | 111 ++++++++++++++++++ 3 files changed, 174 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/utils/logging/TalonFXLogger.java diff --git a/simgui-ds.json b/simgui-ds.json index 172c4e43..7c7465ac 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -13,11 +18,15 @@ { "decKey": 69, "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -92,6 +101,10 @@ "robotJoysticks": [ { "guid": "78696e70757401000000000000000000", + "name": "Keyboard", + "useGamepad": true + }, + { "useGamepad": true }, { diff --git a/simgui.json b/simgui.json index eed2d5d9..7e353f96 100644 --- a/simgui.json +++ b/simgui.json @@ -11,22 +11,69 @@ "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d", "/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/Shooter shoot": "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/Whitelist All": "Command", + "/SmartDashboard/Whitelist Note": "Command", + "/SmartDashboard/gleeb": "Command", + "/SmartDashboard/gloob": "Command" + }, + "windows": { + "/SmartDashboard/Auto Chooser": { + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { "AdvantageKit": { + "RealOutputs": { + "AutoAim": { + "open": true + }, + "open": true + }, "open": true } } }, "NetworkTables Info": { + "AdvantageScope@1": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + } + }, + "Clients": { + "open": true + }, "visible": true } } 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..d8e494a6 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -0,0 +1,111 @@ +package frc.robot.utils.logging; + +import java.nio.ByteBuffer; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; + +/** 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 { + // No position or velocity, use subclasses instead depending on mechanism type + 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 velocityRotationsPerSecond = 0.0; + + public TalonFXLog(double appliedVolts, double statorCurrentAmps, double supplyCurrentAmps, double temperatureCelsius, double position, double velocityRotationsPerSecond) { + this.appliedVolts = appliedVolts; + this.statorCurrentAmps = statorCurrentAmps; + this.supplyCurrentAmps = supplyCurrentAmps; + this.temperatureCelsius = temperatureCelsius; + this.position = position; + this.velocityRotationsPerSecond = velocityRotationsPerSecond; + } + + 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; + } + + @Override + public String getSchema() { + return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity"; + } + + @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(); + return new TalonFXLog(voltage, statorAmps, supplyAmps, temp, rotation, velocity); + } + + @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.velocityRotationsPerSecond); + } + } + } + + 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 rotationSignal; + private final StatusSignal velocitySignal; + + public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOptimizeBusUsage) { + voltageSignal = talon.getMotorVoltage(); + statorCurrentSignal = talon.getStatorCurrent(); + supplyCurrentSignal = talon.getSupplyCurrent(); + temperatureSignal = talon.getDeviceTemp(); + rotationSignal = talon.getPosition(); + velocitySignal = talon.getVelocity(); + + BaseStatusSignal.setUpdateFrequencyForAll(updateFrequencyHz, voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal, rotationSignal, velocitySignal); + if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); + } + + public void update() { + BaseStatusSignal.refreshAll(voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal); + log.appliedVolts = voltageSignal.getValueAsDouble(); + log.statorCurrentAmps = statorCurrentSignal.getValueAsDouble(); + log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); + log.temperatureCelsius = temperatureSignal.getValueAsDouble(); + log.position = rotationSignal.getValueAsDouble(); + log.velocityRotationsPerSecond = rotationSignal.getValueAsDouble(); + } +} From fe923b6ebc771b822f051411295daaa099a099d0 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 28 Feb 2024 11:31:33 -0800 Subject: [PATCH 11/27] refactor swerve to use talonfxlogger --- .../frc/robot/subsystems/swerve/Module.java | 10 +-- .../frc/robot/subsystems/swerve/ModuleIO.java | 14 ++-- .../robot/subsystems/swerve/ModuleIOReal.java | 67 +++++-------------- .../robot/subsystems/swerve/ModuleIOSim.java | 18 ++--- .../robot/utils/logging/TalonFXLogger.java | 12 ++-- 5 files changed, 42 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 1190f4d4..d00c4e14 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -80,7 +80,7 @@ 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())); return optimizedState; } @@ -108,17 +108,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). */ @@ -138,7 +138,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 061b4cfd..9c24a1a7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -14,23 +14,17 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; + 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 driveStatorCurrentAmps = 0.0; - public double driveSupplyCurrentAmps = 0.0; + 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 turnStatorCurrentAmps = 0.0; - public double turnSupplyCurrentAmps = 0.0; + 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 49373d38..be4d204c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -31,6 +31,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.subsystems.swerve.Module.ModuleConstants; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Registration; +import frc.robot.utils.logging.TalonFXLogger; /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and @@ -56,18 +57,14 @@ 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 driveStatorCurrent; - private final StatusSignal driveSupplyCurrent; + private final TalonFXLogger driveLogger; private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnStatorCurrent; - private final StatusSignal turnSupplyCurrent; + private final TalonFXLogger turnLogger; + + // Keep separate for async odometry + StatusSignal drivePosition; + StatusSignal turnPosition; private double lastUpdate = 0; @@ -148,18 +145,13 @@ public ModuleIOReal(ModuleConstants constants) { : SensorDirectionValue.Clockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); - drivePosition = driveTalon.getPosition(); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveStatorCurrent = driveTalon.getStatorCurrent(); - driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveLogger = new TalonFXLogger(driveTalon); turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnLogger = new TalonFXLogger(turnTalon); + + drivePosition = driveTalon.getPosition(); turnPosition = turnTalon.getPosition(); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnStatorCurrent = turnTalon.getStatorCurrent(); - turnSupplyCurrent = turnTalon.getSupplyCurrent(); PhoenixOdometryThread.getInstance() .registerSignals( @@ -170,15 +162,7 @@ public ModuleIOReal(ModuleConstants constants) { Module.ODOMETRY_FREQUENCY_HZ, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, - driveVelocity, - driveAppliedVolts, - driveStatorCurrent, - driveSupplyCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent, - turnSupplyCurrent); + turnAbsolutePosition); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); @@ -186,29 +170,14 @@ public ModuleIOReal(ModuleConstants constants) { @Override public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveStatorCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent); - - inputs.drivePositionMeters = drivePosition.getValueAsDouble(); - inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveStatorCurrentAmps = driveStatorCurrent.getValueAsDouble(); - inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + BaseStatusSignal.refreshAll(turnAbsolutePosition); + driveLogger.update(); + turnLogger.update(); + + inputs.drive = driveLogger.log; inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnStatorCurrentAmps = turnStatorCurrent.getValueAsDouble(); - inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turn = turnLogger.log; var samples = PhoenixOdometryThread.getInstance() diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java index e3308cf3..60b33a03 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java @@ -20,6 +20,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; /** * Physics sim implementation of module IO. @@ -58,22 +59,17 @@ public ModuleIOSim(final String name) { public void updateInputs(final ModuleIOInputs inputs) { 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.driveStatorCurrentAmps = 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.turnStatorCurrentAmps = 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/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index d8e494a6..6f0ceda6 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -21,7 +21,7 @@ public static class TalonFXLog implements StructSerializable { public double supplyCurrentAmps = 0.0; public double temperatureCelsius = 0.0; public double position = 0.0; - public double velocityRotationsPerSecond = 0.0; + public double velocity = 0.0; public TalonFXLog(double appliedVolts, double statorCurrentAmps, double supplyCurrentAmps, double temperatureCelsius, double position, double velocityRotationsPerSecond) { this.appliedVolts = appliedVolts; @@ -29,7 +29,7 @@ public TalonFXLog(double appliedVolts, double statorCurrentAmps, double supplyCu this.supplyCurrentAmps = supplyCurrentAmps; this.temperatureCelsius = temperatureCelsius; this.position = position; - this.velocityRotationsPerSecond = velocityRotationsPerSecond; + this.velocity = velocityRotationsPerSecond; } public static TalonFXLogStruct struct = new TalonFXLogStruct(); @@ -73,7 +73,7 @@ public void pack(ByteBuffer bb, TalonFXLog value) { bb.putDouble(value.supplyCurrentAmps); bb.putDouble(value.temperatureCelsius); bb.putDouble(value.position); - bb.putDouble(value.velocityRotationsPerSecond); + bb.putDouble(value.velocity); } } } @@ -99,6 +99,10 @@ public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOpti if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); } + public TalonFXLogger(TalonFX talon) { + this(talon, 50.0, true); + } + public void update() { BaseStatusSignal.refreshAll(voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal); log.appliedVolts = voltageSignal.getValueAsDouble(); @@ -106,6 +110,6 @@ public void update() { log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); log.temperatureCelsius = temperatureSignal.getValueAsDouble(); log.position = rotationSignal.getValueAsDouble(); - log.velocityRotationsPerSecond = rotationSignal.getValueAsDouble(); + log.velocity = rotationSignal.getValueAsDouble(); } } From 0165ef458c011ffe70cbdb0083cbfc6e628748e6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 28 Feb 2024 11:33:42 -0800 Subject: [PATCH 12/27] run spotless --- .../frc/robot/subsystems/swerve/Module.java | 6 +- .../frc/robot/subsystems/swerve/ModuleIO.java | 1 - .../robot/subsystems/swerve/ModuleIOReal.java | 5 +- .../robot/subsystems/swerve/ModuleIOSim.java | 24 +- .../robot/utils/logging/TalonFXLogger.java | 217 ++++++++++-------- 5 files changed, 140 insertions(+), 113 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index d00c4e14..c9fdb582 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -80,7 +80,11 @@ public SwerveModuleState runSetpoint(SwerveModuleState state) { io.setTurnSetpoint(optimizedState.angle); io.setDriveSetpoint( optimizedState.speedMetersPerSecond - * Math.cos(optimizedState.angle.minus(Rotation2d.fromRotations(inputs.turn.position)).getRadians())); + * Math.cos( + optimizedState + .angle + .minus(Rotation2d.fromRotations(inputs.turn.position)) + .getRadians())); return optimizedState; } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 9c24a1a7..eaeae9b9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; - import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index be4d204c..d01b5c96 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -28,7 +28,6 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import com.google.common.collect.ImmutableSet; 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.utils.logging.TalonFXLogger; @@ -160,9 +159,7 @@ public ModuleIOReal(ModuleConstants constants) { BaseStatusSignal.setUpdateFrequencyForAll( Module.ODOMETRY_FREQUENCY_HZ, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - turnAbsolutePosition); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, turnAbsolutePosition); driveTalon.optimizeBusUtilization(); turnTalon.optimizeBusUtilization(); cancoder.optimizeBusUtilization(); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java index 60b33a03..3da45023 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java @@ -59,17 +59,31 @@ public ModuleIOSim(final String name) { public void updateInputs(final ModuleIOInputs inputs) { driveSim.update(LOOP_PERIOD_SECS); turnSim.update(LOOP_PERIOD_SECS); - - inputs.drive = new TalonFXLog(driveAppliedVolts, driveSim.getCurrentDrawAmps(), driveSim.getCurrentDrawAmps(), 0.0, driveSim.getAngularPositionRad() * Module.WHEEL_RADIUS, driveSim.getAngularVelocityRadPerSec() * Module.WHEEL_RADIUS); + + 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.turn = new TalonFXLog(turnAppliedVolts, turnSim.getCurrentDrawAmps(), turnSim.getCurrentDrawAmps(), 0.0, turnSim.getAngularPositionRad() / (2 * Math.PI), turnSim.getAngularVelocityRadPerSec() / (2 * Math.PI)); - + 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.drive.position}; - inputs.odometryTurnPositions = new Rotation2d[] {Rotation2d.fromRotations(inputs.turn.position)}; + inputs.odometryTurnPositions = + new Rotation2d[] {Rotation2d.fromRotations(inputs.turn.position)}; } @Override diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index 6f0ceda6..134ca81c 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -1,115 +1,128 @@ package frc.robot.utils.logging; -import java.nio.ByteBuffer; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.math.geometry.Rotation2d; 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. -*/ +/** + * 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 { - // No position or velocity, use subclasses instead depending on mechanism type - 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; - - public TalonFXLog(double appliedVolts, double statorCurrentAmps, double supplyCurrentAmps, double temperatureCelsius, double position, double velocityRotationsPerSecond) { - this.appliedVolts = appliedVolts; - this.statorCurrentAmps = statorCurrentAmps; - this.supplyCurrentAmps = supplyCurrentAmps; - this.temperatureCelsius = temperatureCelsius; - this.position = position; - this.velocity = velocityRotationsPerSecond; - } - - 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; - } - - @Override - public String getSchema() { - return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity"; - } - - @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(); - return new TalonFXLog(voltage, statorAmps, supplyAmps, temp, rotation, velocity); - } - - @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); - } - } - } - - 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 rotationSignal; - private final StatusSignal velocitySignal; - - public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOptimizeBusUsage) { - voltageSignal = talon.getMotorVoltage(); - statorCurrentSignal = talon.getStatorCurrent(); - supplyCurrentSignal = talon.getSupplyCurrent(); - temperatureSignal = talon.getDeviceTemp(); - rotationSignal = talon.getPosition(); - velocitySignal = talon.getVelocity(); - - BaseStatusSignal.setUpdateFrequencyForAll(updateFrequencyHz, voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal, rotationSignal, velocitySignal); - if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); - } - - public TalonFXLogger(TalonFX talon) { - this(talon, 50.0, true); + // No position or velocity, use subclasses instead depending on mechanism type + 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; + + public TalonFXLog( + double appliedVolts, + double statorCurrentAmps, + double supplyCurrentAmps, + double temperatureCelsius, + double position, + double velocityRotationsPerSecond) { + this.appliedVolts = appliedVolts; + this.statorCurrentAmps = statorCurrentAmps; + this.supplyCurrentAmps = supplyCurrentAmps; + this.temperatureCelsius = temperatureCelsius; + this.position = position; + this.velocity = velocityRotationsPerSecond; } - public void update() { - BaseStatusSignal.refreshAll(voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal); - log.appliedVolts = voltageSignal.getValueAsDouble(); - log.statorCurrentAmps = statorCurrentSignal.getValueAsDouble(); - log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); - log.temperatureCelsius = temperatureSignal.getValueAsDouble(); - log.position = rotationSignal.getValueAsDouble(); - log.velocity = rotationSignal.getValueAsDouble(); + 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; + } + + @Override + public String getSchema() { + return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity"; + } + + @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(); + return new TalonFXLog(voltage, statorAmps, supplyAmps, temp, rotation, velocity); + } + + @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); + } } + } + + 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 rotationSignal; + private final StatusSignal velocitySignal; + + public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOptimizeBusUsage) { + voltageSignal = talon.getMotorVoltage(); + statorCurrentSignal = talon.getStatorCurrent(); + supplyCurrentSignal = talon.getSupplyCurrent(); + temperatureSignal = talon.getDeviceTemp(); + rotationSignal = talon.getPosition(); + velocitySignal = talon.getVelocity(); + + BaseStatusSignal.setUpdateFrequencyForAll( + updateFrequencyHz, + voltageSignal, + statorCurrentSignal, + supplyCurrentSignal, + temperatureSignal, + rotationSignal, + velocitySignal); + if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); + } + + public TalonFXLogger(TalonFX talon) { + this(talon, 50.0, true); + } + + public void update() { + BaseStatusSignal.refreshAll( + voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal); + log.appliedVolts = voltageSignal.getValueAsDouble(); + log.statorCurrentAmps = statorCurrentSignal.getValueAsDouble(); + log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); + log.temperatureCelsius = temperatureSignal.getValueAsDouble(); + log.position = rotationSignal.getValueAsDouble(); + log.velocity = rotationSignal.getValueAsDouble(); + } } From 22501410277ed0bb9b84f85af37da54e7627ccc9 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 14:28:17 -0700 Subject: [PATCH 13/27] additional merge fixes --- .../java/frc/robot/subsystems/shooter/ShooterSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/swerve/Module.java | 2 +- src/main/java/frc/robot/subsystems/swerve/ModuleIO.java | 2 +- src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java | 4 ++-- src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index f410343f..eae5200e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -164,7 +164,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.pivotStatorCurrentAmps > 40.0) .finallyDo(() -> io.resetPivotPosition(PIVOT_MIN_ANGLE)); } diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 61d274c8..5c935935 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -100,7 +100,7 @@ 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; } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index debe5d28..ce20b2a5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -14,8 +14,8 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import java.util.List; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index a9b36f9e..032c6aa1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -30,8 +30,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.subsystems.swerve.Module.ModuleConstants; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Registration; -import frc.robot.utils.logging.TalonFXLogger; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXLogger; import java.util.List; /** @@ -168,7 +168,7 @@ public ModuleIOReal(ModuleConstants constants) { } @Override - public void updateInputs(ModuleIOInputs inputs) { + public void updateInputs(ModuleIOInputs inputs, final List asyncOdometrySamples) { BaseStatusSignal.refreshAll(turnAbsolutePosition); driveLogger.update(); turnLogger.update(); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java index 31abf1b4..c697cae4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOSim.java @@ -20,8 +20,8 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; import java.util.List; /** From 7d01e14dd8d94027901e963ae9c9e1d833d471cf Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 21:29:38 +0000 Subject: [PATCH 14/27] run spotless --- src/main/java/frc/robot/subsystems/swerve/Module.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 5c935935..73d3bab5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -100,7 +100,11 @@ public SwerveModuleState runVoltageSetpoint(SwerveModuleState state) { io.setTurnSetpoint(optimizedState.angle); io.setDriveVoltage( optimizedState.speedMetersPerSecond - * Math.cos(optimizedState.angle.minus(Rotation2d.fromRotations(inputs.turn.position)).getRadians())); + * Math.cos( + optimizedState + .angle + .minus(Rotation2d.fromRotations(inputs.turn.position)) + .getRadians())); return optimizedState; } From 7ef4cb38f1c6d3c72e9d84a51b8704a1f3703e0e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 14:49:16 -0700 Subject: [PATCH 15/27] refactor shooter to use talonfx logger --- src/main/java/frc/robot/Robot.java | 1 - .../robot/subsystems/shooter/ShooterIO.java | 25 ++--- .../subsystems/shooter/ShooterIOReal.java | 94 +++---------------- .../subsystems/shooter/ShooterIOSim.java | 19 +--- .../subsystems/shooter/ShooterSubsystem.java | 58 +++--------- .../robot/utils/logging/TalonFXLogger.java | 1 - 6 files changed, 41 insertions(+), 157 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 94210365..d4730efe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -346,7 +346,6 @@ public void robotInit() { 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", diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 55f02632..958ba874 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -5,29 +5,18 @@ 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 pivotStatorCurrentAmps = 0.0; - public double pivotSupplyCurrentAmps = 0.0; - public double pivotTempC = 0.0; - - public double flywheelLeftVoltage = 0.0; - public double flywheelLeftVelocityRotationsPerSecond = 0.0; - public double flywheelLeftStatorCurrentAmps = 0.0; - public double flywheelLeftSupplyCurrentAmps = 0.0; - public double flywheelLeftTempC = 0.0; - - public double flywheelRightVoltage = 0.0; - public double flywheelRightVelocityRotationsPerSecond = 0.0; - public double flywheelRightStatorCurrentAmps = 0.0; - public double flywheelRightSupplyCurrentAmps = 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 2e6c619b..fdc50700 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -10,34 +10,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 pivotStatorCurrentAmps = pivotMotor.getStatorCurrent(); - private final StatusSignal pivotSupplyCurrentAmps = pivotMotor.getSupplyCurrent(); - private final StatusSignal pivotTempC = pivotMotor.getDeviceTemp(); - private final StatusSignal pivotRotations = pivotMotor.getPosition(); - - private final StatusSignal flywheelLeftStatorCurrentAmps = - flywheelLeftMotor.getStatorCurrent(); - private final StatusSignal flywheelLeftSupplyCurrentAmps = - flywheelLeftMotor.getSupplyCurrent(); - private final StatusSignal flywheelLeftVoltage = flywheelLeftMotor.getMotorVoltage(); - private final StatusSignal flywheelLeftTempC = flywheelLeftMotor.getDeviceTemp(); - private final StatusSignal flywheelLeftVelocity = flywheelLeftMotor.getVelocity(); - - private final StatusSignal flywheelRightStatorCurrentAmps = - flywheelRightMotor.getStatorCurrent(); - private final StatusSignal flywheelRightSupplyCurrentAmps = - flywheelRightMotor.getSupplyCurrent(); - private final StatusSignal flywheelRightVoltage = flywheelRightMotor.getMotorVoltage(); - private final StatusSignal flywheelRightTempC = flywheelRightMotor.getDeviceTemp(); - private final StatusSignal flywheelRightVelocity = flywheelRightMotor.getVelocity(); + private final TalonFXLogger pivotLogger = new TalonFXLogger(pivotMotor); + + private final TalonFXLogger flywheelLeftLogger = new TalonFXLogger(flywheelLeftMotor); + + 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); @@ -74,14 +58,6 @@ public ShooterIOReal() { pivotMotor.getConfigurator().apply(pivotConfig); pivotMotor.setPosition( ShooterSubsystem.PIVOT_MIN_ANGLE.getRotations()); // Assume we boot at hard stop - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - pivotVelocity, - pivotVoltage, - pivotStatorCurrentAmps, - pivotSupplyCurrentAmps, - pivotTempC, - pivotRotations); pivotMotor.optimizeBusUtilization(); var flywheelConfig = new TalonFXConfiguration(); @@ -103,61 +79,21 @@ public ShooterIOReal() { flywheelLeftMotor.getConfigurator().apply(flywheelConfig); flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; flywheelRightMotor.getConfigurator().apply(flywheelConfig); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - flywheelLeftVelocity, - flywheelLeftVoltage, - flywheelLeftStatorCurrentAmps, - flywheelLeftSupplyCurrentAmps, - flywheelLeftTempC, - flywheelRightVelocity, - flywheelRightVoltage, - flywheelRightStatorCurrentAmps, - flywheelRightSupplyCurrentAmps, - flywheelRightTempC); flywheelLeftMotor.optimizeBusUtilization(); flywheelRightMotor.optimizeBusUtilization(); } @Override public void updateInputs(ShooterIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll( - pivotRotations, - pivotVelocity, - pivotVoltage, - pivotStatorCurrentAmps, - pivotSupplyCurrentAmps, - pivotTempC, - flywheelLeftVelocity, - flywheelLeftVoltage, - flywheelLeftStatorCurrentAmps, - flywheelLeftSupplyCurrentAmps, - flywheelLeftTempC, - flywheelRightVelocity, - flywheelRightVoltage, - flywheelRightStatorCurrentAmps, - flywheelRightSupplyCurrentAmps, - flywheelRightTempC); - - inputs.pivotRotation = Rotation2d.fromRotations(pivotRotations.getValueAsDouble()); - inputs.pivotVelocityRotationsPerSecond = pivotVelocity.getValueAsDouble(); - inputs.pivotVoltage = pivotVoltage.getValueAsDouble(); - inputs.pivotStatorCurrentAmps = pivotStatorCurrentAmps.getValueAsDouble(); - inputs.pivotSupplyCurrentAmps = pivotSupplyCurrentAmps.getValueAsDouble(); - inputs.pivotTempC = pivotTempC.getValueAsDouble(); - - inputs.flywheelLeftVelocityRotationsPerSecond = flywheelLeftVelocity.getValueAsDouble(); - inputs.flywheelLeftVoltage = flywheelLeftVoltage.getValueAsDouble(); - inputs.flywheelLeftStatorCurrentAmps = flywheelLeftStatorCurrentAmps.getValueAsDouble(); - inputs.flywheelLeftSupplyCurrentAmps = flywheelLeftSupplyCurrentAmps.getValueAsDouble(); - inputs.flywheelLeftTempC = flywheelLeftTempC.getValueAsDouble(); - - inputs.flywheelRightVelocityRotationsPerSecond = flywheelRightVelocity.getValueAsDouble(); - inputs.flywheelRightVoltage = flywheelRightVoltage.getValueAsDouble(); - inputs.flywheelRightStatorCurrentAmps = flywheelRightStatorCurrentAmps.getValueAsDouble(); - inputs.flywheelRightSupplyCurrentAmps = flywheelRightSupplyCurrentAmps.getValueAsDouble(); - inputs.flywheelRightTempC = flywheelRightTempC.getValueAsDouble(); + pivotLogger.update(); + flywheelLeftLogger.update(); + flywheelRightLogger.update(); + + inputs.pivot = pivotLogger.log; + + inputs.leftFlywheel = flywheelLeftLogger.log; + + inputs.rightFlywheel = flywheelRightLogger.log; } 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 c6781977..1c6612d8 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,11 @@ 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.pivotStatorCurrentAmps = 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.flywheelLeftStatorCurrentAmps = 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.flywheelRightStatorCurrentAmps = 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 eae5200e..24ad2eda 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -79,18 +79,18 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); - shooterLig.setAngle(inputs.pivotRotation.unaryMinus().minus(Rotation2d.fromDegrees(180.0))); + 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,19 +105,19 @@ public Command runStateCmd( Logger.recordOutput( "Shooter/Left At Target", MathUtil.isNear( - left.getAsDouble(), inputs.flywheelLeftVelocityRotationsPerSecond, 1.0)); + left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Right At Target", MathUtil.isNear( - right.getAsDouble(), inputs.flywheelRightVelocityRotationsPerSecond, 1.0)); + 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)); + rotation.get().getDegrees(), (inputs.pivot.position * 360.0), 0.25)); io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble()); io.setPivotSetpoint(rotation.get()); }); @@ -134,16 +134,16 @@ public Command runFlywheelsCmd(DoubleSupplier left, DoubleSupplier right) { Logger.recordOutput( "Shooter/Left At Target", MathUtil.isNear( - left.getAsDouble(), inputs.flywheelLeftVelocityRotationsPerSecond, 1.0)); + left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Right At Target", MathUtil.isNear( - right.getAsDouble(), inputs.flywheelRightVelocityRotationsPerSecond, 1.0)); + 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); @@ -164,7 +164,7 @@ public Command runFlywheelVoltageCmd(Rotation2d rotation, double voltage) { public Command runPivotCurrentZeroing() { return this.run(() -> io.setPivotVoltage(-1.0)) - .until(() -> inputs.pivotStatorCurrentAmps > 40.0) + .until(() -> inputs.pivot.statorCurrentAmps > 40.0) .finallyDo(() -> io.resetPivotPosition(PIVOT_MIN_ANGLE)); } @@ -185,41 +185,13 @@ 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)); } 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); + 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); } } diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index 134ca81c..79e506c0 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -12,7 +12,6 @@ * rotations or meters, depending on how the mechanism is configured. */ public class TalonFXLogger { - // No position or velocity, use subclasses instead depending on mechanism type public static class TalonFXLog implements StructSerializable { public double appliedVolts = 0.0; public double statorCurrentAmps = 0.0; From e1ac136a1d5e03a15f6cd92323fdfb5396ee07ff Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 15:00:12 -0700 Subject: [PATCH 16/27] make intake use talonfxlogger --- .../frc/robot/subsystems/intake/IntakeIO.java | 17 +++--- .../robot/subsystems/intake/IntakeIOReal.java | 53 +++---------------- .../robot/utils/logging/TalonFXLogger.java | 5 +- 3 files changed, 16 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index ddf9532c..2acb9ff6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -6,20 +6,15 @@ import org.littletonrobotics.junction.AutoLog; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; + public interface IntakeIO { @AutoLog public static class IntakeIOInputs { - public double intakeVelocityRotationsPerSecond = 0.0; - public double intakeAppliedVolts = 0.0; - public double intakeStatorCurrentAmps = 0.0; - public double intakeSupplyCurrentAmps = 0.0; - public double intakeTemperatureCelsius = 0.0; - - public double centeringVelocityRotationsPerSecond = 0.0; - public double centeringAppliedVolts = 0.0; - public double centeringStatorCurrentAmps = 0.0; - public double centeringSupplyCurrentAmps = 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 4455c98d..a32d6a6e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -12,6 +12,8 @@ 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 { private final TalonFX intakeMotor = new TalonFX(14, "canivore"); @@ -24,17 +26,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 intakeStatorCurrentAmps = intakeMotor.getStatorCurrent(); - private final StatusSignal intakeSupplyCurrentAmps = intakeMotor.getSupplyCurrent(); - 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 centeringStatorCurrentAmps = centeringMotor.getStatorCurrent(); - private final StatusSignal centeringSupplyCurrentAmps = centeringMotor.getSupplyCurrent(); - private final StatusSignal centeringTemp = centeringMotor.getDeviceTemp(); + private final TalonFXLogger centeringLogger = new TalonFXLogger(centeringMotor); public IntakeIOReal() { var intakeConfig = new TalonFXConfiguration(); @@ -58,48 +52,13 @@ public IntakeIOReal() { centeringConfig.Slot0.kP = 0.1; centeringMotor.getConfigurator().apply(centeringConfig); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - intakeVelocity, - intakeVoltage, - intakeStatorCurrentAmps, - intakeSupplyCurrentAmps, - intakeTemp, - centeringVelocity, - centeringVoltage, - centeringStatorCurrentAmps, - centeringSupplyCurrentAmps, - centeringTemp); - intakeMotor.optimizeBusUtilization(); - centeringMotor.optimizeBusUtilization(); } /** Updates the set of loggable inputs. */ @Override public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll( - intakeVelocity, - intakeVoltage, - intakeStatorCurrentAmps, - intakeSupplyCurrentAmps, - intakeTemp, - centeringVelocity, - centeringVoltage, - centeringStatorCurrentAmps, - centeringSupplyCurrentAmps, - centeringTemp); - inputs.intakeVelocityRotationsPerSecond = intakeVelocity.getValueAsDouble(); - inputs.intakeAppliedVolts = intakeVoltage.getValueAsDouble(); - inputs.intakeStatorCurrentAmps = intakeStatorCurrentAmps.getValueAsDouble(); - inputs.intakeSupplyCurrentAmps = intakeSupplyCurrentAmps.getValueAsDouble(); - inputs.intakeTemperatureCelsius = intakeTemp.getValueAsDouble(); - - inputs.centeringVelocityRotationsPerSecond = centeringVelocity.getValueAsDouble(); - inputs.centeringAppliedVolts = centeringVoltage.getValueAsDouble(); - inputs.centeringStatorCurrentAmps = centeringStatorCurrentAmps.getValueAsDouble(); - inputs.centeringSupplyCurrentAmps = centeringSupplyCurrentAmps.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/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index 79e506c0..f5dd83a2 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -12,6 +12,7 @@ * 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; @@ -114,7 +115,7 @@ public TalonFXLogger(TalonFX talon) { this(talon, 50.0, true); } - public void update() { + public TalonFXLog update() { BaseStatusSignal.refreshAll( voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal); log.appliedVolts = voltageSignal.getValueAsDouble(); @@ -123,5 +124,7 @@ public void update() { log.temperatureCelsius = temperatureSignal.getValueAsDouble(); log.position = rotationSignal.getValueAsDouble(); log.velocity = rotationSignal.getValueAsDouble(); + + return log; } } From 6a8aa1fa7e77c515e406542fa92d40066a122265 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 15:09:51 -0700 Subject: [PATCH 17/27] refactor feeder and elevator --- .../robot/subsystems/elevator/ElevatorIO.java | 10 ++-- .../subsystems/elevator/ElevatorIOReal.java | 48 +++---------------- .../subsystems/elevator/ElevatorIOSim.java | 7 +-- .../elevator/ElevatorSubsystem.java | 37 ++------------ .../frc/robot/subsystems/feeder/FeederIO.java | 8 ++-- .../robot/subsystems/feeder/FeederIOReal.java | 19 ++------ .../subsystems/shooter/ShooterIOReal.java | 10 ++-- .../robot/subsystems/swerve/ModuleIOReal.java | 6 +-- 8 files changed, 26 insertions(+), 119 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 905541ff..06f30c15 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -6,15 +6,13 @@ import org.littletonrobotics.junction.AutoLog; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; + 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[] elevatorStatorCurrentAmps = new double[] {}; - public double[] elevatorSupplyCurrentAmps = 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 9d1249ec..c12fb602 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -15,6 +15,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.utils.logging.TalonFXLogger; + /** Elevator IO using TalonFXs. */ public class ElevatorIOReal implements ElevatorIO { private final TalonFX motor = new TalonFX(16, "canivore"); @@ -24,16 +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 statorCurrent = motor.getStatorCurrent(); - private final StatusSignal supplyCurrent = motor.getSupplyCurrent(); - private final StatusSignal temp = motor.getDeviceTemp(); + private final TalonFXLogger leaderLogger = new TalonFXLogger(motor); - private final StatusSignal followerStatorCurrent = follower.getStatorCurrent(); - private final StatusSignal followerSupplyCurrent = follower.getSupplyCurrent(); - private final StatusSignal followerTemp = follower.getDeviceTemp(); + private final TalonFXLogger followerLogger = new TalonFXLogger(follower); public ElevatorIOReal() { var config = new TalonFXConfiguration(); @@ -66,43 +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, - statorCurrent, - supplyCurrent, - temp, - followerStatorCurrent, - followerSupplyCurrent, - followerTemp); - motor.optimizeBusUtilization(); - follower.optimizeBusUtilization(); } @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll( - position, - velocity, - voltage, - statorCurrent, - supplyCurrent, - temp, - followerStatorCurrent, - followerSupplyCurrent, - followerTemp); - inputs.elevatorPositionMeters = position.getValueAsDouble(); - inputs.elevatorVelocityMetersPerSec = velocity.getValueAsDouble(); - inputs.elevatorAppliedVolts = voltage.getValueAsDouble(); - inputs.elevatorStatorCurrentAmps = - new double[] {statorCurrent.getValueAsDouble(), followerStatorCurrent.getValueAsDouble()}; - inputs.elevatorSupplyCurrentAmps = - new double[] {supplyCurrent.getValueAsDouble(), followerSupplyCurrent.getValueAsDouble()}; - inputs.elevatorTempCelsius = - new double[] {temp.getValueAsDouble(), followerTemp.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 efe6643d..7320253e 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,7 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { stop(); } physicsSim.update(0.020); - inputs.elevatorPositionMeters = physicsSim.getPositionMeters(); - inputs.elevatorVelocityMetersPerSec = physicsSim.getVelocityMetersPerSecond(); - inputs.elevatorAppliedVolts = volts; - inputs.elevatorStatorCurrentAmps = 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 cf6d18f1..95688687 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -17,10 +17,8 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj2.command.Command; -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 java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -75,7 +73,7 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); - carriage.setLength(inputs.elevatorPositionMeters); + carriage.setLength(inputs.leader.position); Logger.recordOutput("Elevator/Mechanism2d", mech2d); Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose()); @@ -91,39 +89,10 @@ public Command setExtensionCmd(DoubleSupplier meters) { public Command runCurrentZeroing() { return this.run(() -> io.setVoltage(-1.0)) - .until(() -> inputs.elevatorStatorCurrentAmps[0] > 40.0) + .until(() -> inputs.leader.statorCurrentAmps > 40.0) .finallyDo(() -> io.resetEncoder(0.0)); } - 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(), @@ -143,6 +112,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 56fe1978..9e73503b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -6,14 +6,12 @@ import org.littletonrobotics.junction.AutoLog; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; + public interface FeederIO { @AutoLog public static class FeederIOInputs { - public double feederVelocityRotationsPerSec = 0.0; - public double feederAppliedVolts = 0.0; - public double feederStatorCurrentAmps = 0.0; - public double feederSupplyCurrentAmps = 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 170e4599..77af2bed 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -12,6 +12,7 @@ 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,11 +21,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 statorCurrent = motor.getStatorCurrent(); - private final StatusSignal supplyCurrent = motor.getSupplyCurrent(); - 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); @@ -41,21 +38,11 @@ public FeederIOReal() { config.Slot0.kP = 0.1; motor.getConfigurator().apply(config); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, velocity, voltage, statorCurrent, temp, supplyCurrent); - motor.optimizeBusUtilization(); } @Override public void updateInputs(final FeederIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp); - - inputs.feederVelocityRotationsPerSec = velocity.getValueAsDouble(); - inputs.feederAppliedVolts = voltage.getValueAsDouble(); - inputs.feederStatorCurrentAmps = statorCurrent.getValueAsDouble(); - inputs.feederSupplyCurrentAmps = supplyCurrent.getValueAsDouble(); - inputs.feederTempC = temp.getValueAsDouble(); + inputs.feeder = logger.update(); inputs.firstBeambreak = firstBeambreak.get(); inputs.lastBeambreak = lastBeambreak.get(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index fdc50700..975cdd20 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -85,15 +85,11 @@ public ShooterIOReal() { @Override public void updateInputs(ShooterIOInputsAutoLogged inputs) { - pivotLogger.update(); - flywheelLeftLogger.update(); - flywheelRightLogger.update(); + inputs.pivot = pivotLogger.update(); - inputs.pivot = pivotLogger.log; + inputs.leftFlywheel = flywheelLeftLogger.update(); - inputs.leftFlywheel = flywheelLeftLogger.log; - - inputs.rightFlywheel = flywheelRightLogger.log; + inputs.rightFlywheel = flywheelRightLogger.update(); } public void setPivotVoltage(final double voltage) { diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index 032c6aa1..b9f87955 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -170,13 +170,11 @@ public ModuleIOReal(ModuleConstants constants) { @Override public void updateInputs(ModuleIOInputs inputs, final List asyncOdometrySamples) { BaseStatusSignal.refreshAll(turnAbsolutePosition); - driveLogger.update(); - turnLogger.update(); - inputs.drive = driveLogger.log; + inputs.drive = driveLogger.update(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turn = turnLogger.log; + inputs.turn = turnLogger.update(); inputs.odometryTimestamps = asyncOdometrySamples.stream().mapToDouble(s -> s.timestamp()).toArray(); From 3e54f14e5b049369d02169606f4afe28cc20e29a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 15:13:21 -0700 Subject: [PATCH 18/27] refactor carriage --- src/main/java/frc/robot/Robot.java | 1 - .../robot/subsystems/carriage/CarriageIO.java | 8 +++---- .../subsystems/carriage/CarriageIOReal.java | 23 +++++-------------- .../robot/subsystems/climber/ClimberIO.java | 1 + 4 files changed, 10 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d4730efe..ff916efd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -345,7 +345,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 Flywheel Sysid", shooter.runFlywheelSysidCmd()); SmartDashboard.putData( "manual zero shooter", diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java index ccbe6347..3cd40ba9 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java @@ -6,14 +6,12 @@ import org.littletonrobotics.junction.AutoLog; +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; + public interface CarriageIO { @AutoLog public static class CarriageIOInputs { - public double velocityRotationsPerSecond = 0.0; - public double appliedVolts = 0.0; - public double statorCurrentAmps = 0.0; - public double supplyCurrentAmps = 0.0; - public double temperatureCelsius = 0.0; + 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 e3fc2f6e..5dd9f077 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java @@ -11,20 +11,17 @@ 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 statorCurrent = motor.getStatorCurrent(); - final StatusSignal supplyCurrent = motor.getSupplyCurrent(); - final StatusSignal temp = motor.getDeviceTemp(); + private final TalonFXLogger logger = new TalonFXLogger(motor); public CarriageIOReal() { var config = new TalonFXConfiguration(); @@ -32,20 +29,12 @@ public CarriageIOReal() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; motor.getConfigurator().apply(config); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, velocity, voltage, statorCurrent, supplyCurrent, temp); - motor.optimizeBusUtilization(); } /** Updates the set of loggable inputs. */ @Override public void updateInputs(final CarriageIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp); - inputs.velocityRotationsPerSecond = velocity.getValueAsDouble(); - inputs.appliedVolts = voltage.getValueAsDouble(); - inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); - inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); - inputs.temperatureCelsius = temp.getValueAsDouble(); + inputs.carriage = logger.update(); inputs.beambreak = beambreak.get(); } 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; From efac18e856cf8703d98bca948d918282e44a2978 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 16:05:25 -0700 Subject: [PATCH 19/27] check for license faults --- .../robot/utils/logging/TalonFXLogger.java | 46 ++++++++++++++----- 1 file changed, 35 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index f5dd83a2..f80a20e0 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -21,19 +21,35 @@ public static class TalonFXLog implements StructSerializable { public double position = 0.0; public double velocity = 0.0; + // Faults + public boolean licenseFault = false; + public TalonFXLog( double appliedVolts, double statorCurrentAmps, double supplyCurrentAmps, double temperatureCelsius, double position, - double velocityRotationsPerSecond) { + double velocityRotationsPerSecond, + boolean licenseFault) { this.appliedVolts = appliedVolts; this.statorCurrentAmps = statorCurrentAmps; this.supplyCurrentAmps = supplyCurrentAmps; this.temperatureCelsius = temperatureCelsius; this.position = position; this.velocity = velocityRotationsPerSecond; + + licenseFault = false; + } + + public TalonFXLog( + double appliedVolts, + double statorCurrentAmps, + double supplyCurrentAmps, + double temperatureCelsius, + double position, + double velocityRotationsPerSecond) { + this(appliedVolts, statorCurrentAmps, supplyCurrentAmps, temperatureCelsius, position, velocityRotationsPerSecond, false); } public static TalonFXLogStruct struct = new TalonFXLogStruct(); @@ -52,12 +68,12 @@ public String getTypeString() { @Override public int getSize() { - return kSizeDouble * 6; + return kSizeDouble * 6 + kSizeBool * 1; } @Override public String getSchema() { - return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity"; + return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity;boolean licenseFault"; } @Override @@ -68,7 +84,8 @@ public TalonFXLog unpack(ByteBuffer bb) { double temp = bb.getDouble(); double rotation = bb.getDouble(); double velocity = bb.getDouble(); - return new TalonFXLog(voltage, statorAmps, supplyAmps, temp, rotation, velocity); + boolean licenseFault = bb.get() != 0; + return new TalonFXLog(voltage, statorAmps, supplyAmps, temp, rotation, velocity, licenseFault); } @Override @@ -79,6 +96,7 @@ public void pack(ByteBuffer bb, TalonFXLog value) { bb.putDouble(value.temperatureCelsius); bb.putDouble(value.position); bb.putDouble(value.velocity); + bb.put(value.licenseFault ? (byte)0 : (byte)1); } } } @@ -89,25 +107,30 @@ public void pack(ByteBuffer bb, TalonFXLog value) { private final StatusSignal statorCurrentSignal; private final StatusSignal supplyCurrentSignal; private final StatusSignal temperatureSignal; - private final StatusSignal rotationSignal; + private final StatusSignal positionSignal; private final StatusSignal velocitySignal; + private final StatusSignal licenseFaultSignal; + public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOptimizeBusUsage) { voltageSignal = talon.getMotorVoltage(); statorCurrentSignal = talon.getStatorCurrent(); supplyCurrentSignal = talon.getSupplyCurrent(); temperatureSignal = talon.getDeviceTemp(); - rotationSignal = talon.getPosition(); + positionSignal = talon.getPosition(); velocitySignal = talon.getVelocity(); + licenseFaultSignal = talon.getFault_UnlicensedFeatureInUse(); + BaseStatusSignal.setUpdateFrequencyForAll( updateFrequencyHz, voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal, - rotationSignal, - velocitySignal); + positionSignal, + velocitySignal, + licenseFaultSignal); if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); } @@ -117,13 +140,14 @@ public TalonFXLogger(TalonFX talon) { public TalonFXLog update() { BaseStatusSignal.refreshAll( - voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal); + voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal, positionSignal, velocitySignal); log.appliedVolts = voltageSignal.getValueAsDouble(); log.statorCurrentAmps = statorCurrentSignal.getValueAsDouble(); log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); log.temperatureCelsius = temperatureSignal.getValueAsDouble(); - log.position = rotationSignal.getValueAsDouble(); - log.velocity = rotationSignal.getValueAsDouble(); + log.position = positionSignal.getValueAsDouble(); + log.velocity = velocitySignal.getValueAsDouble(); + log.licenseFault = licenseFaultSignal.getValue(); return log; } From f511a581e8051b03148658a6ee866cbd37946dea Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 17 Mar 2024 16:13:36 -0700 Subject: [PATCH 20/27] independantly configure fault update rate --- src/main/java/frc/robot/utils/logging/TalonFXLogger.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index f80a20e0..0dd26c68 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -112,7 +112,7 @@ public void pack(ByteBuffer bb, TalonFXLog value) { private final StatusSignal licenseFaultSignal; - public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOptimizeBusUsage) { + public TalonFXLogger(TalonFX talon, double updateFrequencyHz, double faultFrequencyHz, boolean shouldOptimizeBusUsage) { voltageSignal = talon.getMotorVoltage(); statorCurrentSignal = talon.getStatorCurrent(); supplyCurrentSignal = talon.getSupplyCurrent(); @@ -129,13 +129,13 @@ public TalonFXLogger(TalonFX talon, double updateFrequencyHz, boolean shouldOpti supplyCurrentSignal, temperatureSignal, positionSignal, - velocitySignal, - licenseFaultSignal); + velocitySignal); + BaseStatusSignal.setUpdateFrequencyForAll(faultFrequencyHz, licenseFaultSignal); if (shouldOptimizeBusUsage) talon.optimizeBusUtilization(); } public TalonFXLogger(TalonFX talon) { - this(talon, 50.0, true); + this(talon, 50.0, 1.0, true); } public TalonFXLog update() { From 6de99c061a29676acf1beedf2e88d58ffe7ecf65 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Mon, 18 Mar 2024 06:05:47 +0000 Subject: [PATCH 21/27] run spotless --- .../robot/subsystems/carriage/CarriageIO.java | 3 +-- .../subsystems/carriage/CarriageIOReal.java | 2 -- .../robot/subsystems/elevator/ElevatorIO.java | 3 +-- .../subsystems/elevator/ElevatorIOReal.java | 3 --- .../subsystems/elevator/ElevatorIOSim.java | 9 ++++++- .../frc/robot/subsystems/feeder/FeederIO.java | 3 +-- .../robot/subsystems/feeder/FeederIOReal.java | 2 -- .../frc/robot/subsystems/intake/IntakeIO.java | 4 +-- .../robot/subsystems/intake/IntakeIOReal.java | 3 --- .../robot/subsystems/shooter/ShooterIO.java | 1 - .../subsystems/shooter/ShooterIOReal.java | 2 -- .../subsystems/shooter/ShooterIOSim.java | 27 ++++++++++++++++--- .../subsystems/shooter/ShooterSubsystem.java | 25 ++++++++--------- .../robot/utils/logging/TalonFXLogger.java | 27 +++++++++++++++---- 14 files changed, 71 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java index 3cd40ba9..4a67c0af 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIO.java @@ -4,9 +4,8 @@ package frc.robot.subsystems.carriage; -import org.littletonrobotics.junction.AutoLog; - import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; +import org.littletonrobotics.junction.AutoLog; public interface CarriageIO { @AutoLog diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java index 5dd9f077..778bf713 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java @@ -4,8 +4,6 @@ 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; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 06f30c15..125ccb0b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -4,9 +4,8 @@ package frc.robot.subsystems.elevator; -import org.littletonrobotics.junction.AutoLog; - import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; +import org.littletonrobotics.junction.AutoLog; public interface ElevatorIO { @AutoLog diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index c12fb602..27b66a21 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; @@ -14,7 +12,6 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import frc.robot.utils.logging.TalonFXLogger; /** Elevator IO using TalonFXs. */ diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 7320253e..7a8fe555 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -37,7 +37,14 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { stop(); } physicsSim.update(0.020); - inputs.leader = new TalonFXLog(volts, physicsSim.getCurrentDrawAmps(), physicsSim.getCurrentDrawAmps(), 0.0, physicsSim.getPositionMeters(), physicsSim.getVelocityMetersPerSecond()); + 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/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index 9e73503b..74765709 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -4,9 +4,8 @@ package frc.robot.subsystems.feeder; -import org.littletonrobotics.junction.AutoLog; - import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; +import org.littletonrobotics.junction.AutoLog; public interface FeederIO { @AutoLog diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java index 77af2bed..b7acbaa2 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java @@ -4,8 +4,6 @@ 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; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 2acb9ff6..86f027c0 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -4,9 +4,8 @@ package frc.robot.subsystems.intake; -import org.littletonrobotics.junction.AutoLog; - import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; +import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { @AutoLog @@ -14,7 +13,6 @@ public static class IntakeIOInputs { 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 a32d6a6e..59c688c3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -4,14 +4,11 @@ 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. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 958ba874..e24b9e48 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; - import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 975cdd20..ecdf0ac5 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; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 1c6612d8..48dce3df 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -49,11 +49,32 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) { rightFlywheelSim.update(0.020); pivotSim.update(0.020); - inputs.pivot = new TalonFXLog(0.0, pivotSim.getCurrentDrawAmps(), pivotSim.getCurrentDrawAmps(), 0.0, pivotSim.getAngleRads() / (2 * Math.PI), pivotSim.getVelocityRadPerSec() / (2 * Math.PI)); + inputs.pivot = + new TalonFXLog( + 0.0, + pivotSim.getCurrentDrawAmps(), + pivotSim.getCurrentDrawAmps(), + 0.0, + pivotSim.getAngleRads() / (2 * Math.PI), + pivotSim.getVelocityRadPerSec() / (2 * Math.PI)); - inputs.leftFlywheel = new TalonFXLog(0.0, leftFlywheelSim.getCurrentDrawAmps(), leftFlywheelSim.getCurrentDrawAmps(), 0.0, leftFlywheelSim.getAngularPositionRotations(), leftFlywheelSim.getAngularVelocityRPM() / 60.0); + inputs.leftFlywheel = + new TalonFXLog( + 0.0, + leftFlywheelSim.getCurrentDrawAmps(), + leftFlywheelSim.getCurrentDrawAmps(), + 0.0, + leftFlywheelSim.getAngularPositionRotations(), + leftFlywheelSim.getAngularVelocityRPM() / 60.0); - inputs.rightFlywheel = new TalonFXLog(0.0, rightFlywheelSim.getCurrentDrawAmps(), rightFlywheelSim.getCurrentDrawAmps(), 0.0, rightFlywheelSim.getAngularPositionRotations(), rightFlywheelSim.getAngularVelocityRPM() / 60.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 24ad2eda..cf30357a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -79,14 +79,20 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); - shooterLig.setAngle(Rotation2d.fromRotations(inputs.pivot.position).unaryMinus().minus(Rotation2d.fromDegrees(180.0))); + 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, Rotation2d.fromRotations(inputs.pivot.position).getRadians(), 0.0)); + 0.0437896, + 0.0, + 0.3274568, + new Rotation3d(0.0, Rotation2d.fromRotations(inputs.pivot.position).getRadians(), 0.0)); } public Rotation2d getAngle() { @@ -104,20 +110,17 @@ public Command runStateCmd( Logger.recordOutput("Shooter/Right Velocity Setpoint", right.getAsDouble()); Logger.recordOutput( "Shooter/Left At Target", - MathUtil.isNear( - left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); + MathUtil.isNear(left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Right At Target", - MathUtil.isNear( - right.getAsDouble(), inputs.rightFlywheel.velocity, 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.pivot.position * 360.0) - rotation.get().getDegrees()); Logger.recordOutput( "Shooter/Pivot At Target", - MathUtil.isNear( - rotation.get().getDegrees(), (inputs.pivot.position * 360.0), 0.25)); + MathUtil.isNear(rotation.get().getDegrees(), (inputs.pivot.position * 360.0), 0.25)); io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble()); io.setPivotSetpoint(rotation.get()); }); @@ -133,12 +136,10 @@ 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.leftFlywheel.velocity, 1.0)); + MathUtil.isNear(left.getAsDouble(), inputs.leftFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Right At Target", - MathUtil.isNear( - right.getAsDouble(), inputs.rightFlywheel.velocity, 1.0)); + MathUtil.isNear(right.getAsDouble(), inputs.rightFlywheel.velocity, 1.0)); Logger.recordOutput( "Shooter/Pivot At Target", MathUtil.isNear( diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index 0dd26c68..8e45075f 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -49,7 +49,14 @@ public TalonFXLog( double temperatureCelsius, double position, double velocityRotationsPerSecond) { - this(appliedVolts, statorCurrentAmps, supplyCurrentAmps, temperatureCelsius, position, velocityRotationsPerSecond, false); + this( + appliedVolts, + statorCurrentAmps, + supplyCurrentAmps, + temperatureCelsius, + position, + velocityRotationsPerSecond, + false); } public static TalonFXLogStruct struct = new TalonFXLogStruct(); @@ -85,7 +92,8 @@ public TalonFXLog unpack(ByteBuffer bb) { double rotation = bb.getDouble(); double velocity = bb.getDouble(); boolean licenseFault = bb.get() != 0; - return new TalonFXLog(voltage, statorAmps, supplyAmps, temp, rotation, velocity, licenseFault); + return new TalonFXLog( + voltage, statorAmps, supplyAmps, temp, rotation, velocity, licenseFault); } @Override @@ -96,7 +104,7 @@ public void pack(ByteBuffer bb, TalonFXLog value) { bb.putDouble(value.temperatureCelsius); bb.putDouble(value.position); bb.putDouble(value.velocity); - bb.put(value.licenseFault ? (byte)0 : (byte)1); + bb.put(value.licenseFault ? (byte) 0 : (byte) 1); } } } @@ -112,7 +120,11 @@ public void pack(ByteBuffer bb, TalonFXLog value) { private final StatusSignal licenseFaultSignal; - public TalonFXLogger(TalonFX talon, double updateFrequencyHz, double faultFrequencyHz, boolean shouldOptimizeBusUsage) { + public TalonFXLogger( + TalonFX talon, + double updateFrequencyHz, + double faultFrequencyHz, + boolean shouldOptimizeBusUsage) { voltageSignal = talon.getMotorVoltage(); statorCurrentSignal = talon.getStatorCurrent(); supplyCurrentSignal = talon.getSupplyCurrent(); @@ -140,7 +152,12 @@ public TalonFXLogger(TalonFX talon) { public TalonFXLog update() { BaseStatusSignal.refreshAll( - voltageSignal, statorCurrentSignal, supplyCurrentSignal, temperatureSignal, positionSignal, velocitySignal); + voltageSignal, + statorCurrentSignal, + supplyCurrentSignal, + temperatureSignal, + positionSignal, + velocitySignal); log.appliedVolts = voltageSignal.getValueAsDouble(); log.statorCurrentAmps = statorCurrentSignal.getValueAsDouble(); log.supplyCurrentAmps = supplyCurrentSignal.getValueAsDouble(); From 93c711fa00312b4f947ff11f1b05315187140413 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Mon, 18 Mar 2024 12:25:50 -0700 Subject: [PATCH 22/27] add talonfxfaultmanager --- src/main/java/frc/robot/Robot.java | 4 ++ .../carriage/CarriageSubsystem.java | 3 + .../elevator/ElevatorSubsystem.java | 4 ++ .../subsystems/feeder/FeederSubsystem.java | 3 + .../subsystems/intake/IntakeSubsystem.java | 4 ++ .../subsystems/shooter/ShooterSubsystem.java | 5 ++ .../frc/robot/subsystems/swerve/Module.java | 4 ++ .../utils/logging/TalonFXFaultManager.java | 61 +++++++++++++++++++ 8 files changed, 88 insertions(+) create mode 100644 src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ff916efd..6a9d41d9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,6 +48,8 @@ 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; @@ -354,6 +356,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/CarriageSubsystem.java b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java index bad032c2..d721cb99 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java @@ -7,6 +7,8 @@ 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 +27,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/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 95688687..df1257e2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -19,6 +19,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.utils.logging.TalonFXFaultManager; + import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -72,6 +74,8 @@ 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.leader.position); Logger.recordOutput("Elevator/Mechanism2d", mech2d); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java index 0ec6e7cb..c3eeafe1 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java @@ -6,6 +6,8 @@ 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 +27,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/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 8c90b0f2..513c4d74 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -6,6 +6,8 @@ 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 +24,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/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 24ad2eda..5c7a6dce 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -19,6 +19,8 @@ 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; @@ -78,6 +80,9 @@ public ShooterSubsystem(ShooterIO shooterIO) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); + 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); diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 73d3bab5..cf5b596e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -18,6 +18,8 @@ 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; @@ -60,6 +62,8 @@ public void updateInputs(final List asyncOdometrySamples) { public void periodic() { Logger.processInputs(String.format("Swerve/%s Module", io.getModuleName()), inputs); + 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 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..3f17340a --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java @@ -0,0 +1,61 @@ +// 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 java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import org.littletonrobotics.junction.Logger; + +import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; + +/** 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; + } +} From 2b2374961b8227754073a9db816f60eaacc2c93d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Mon, 18 Mar 2024 23:13:44 -0700 Subject: [PATCH 23/27] show fault on leds while disabled --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6a9d41d9..fa5336b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -198,7 +198,7 @@ public void robotInit() { shooter.runFlywheelsCmd(() -> flywheelIdleSpeed, () -> flywheelIdleSpeed)); 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)); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java index 00df533e..5114586a 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java @@ -88,7 +88,7 @@ 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 +107,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), From fedf6b94b718ea5c7cc75303c3d7032f56017b79 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 19 Mar 2024 06:15:09 +0000 Subject: [PATCH 24/27] run spotless --- src/main/java/frc/robot/Robot.java | 5 +- .../carriage/CarriageSubsystem.java | 1 - .../elevator/ElevatorSubsystem.java | 1 - .../subsystems/feeder/FeederSubsystem.java | 1 - .../subsystems/intake/IntakeSubsystem.java | 1 - .../robot/subsystems/leds/LEDSubsystem.java | 3 +- .../subsystems/shooter/ShooterSubsystem.java | 1 - .../frc/robot/subsystems/swerve/Module.java | 7 +- .../utils/logging/TalonFXFaultManager.java | 88 +++++++++---------- 9 files changed, 52 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fa5336b9..be7a6587 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -49,7 +49,6 @@ 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; @@ -198,7 +197,9 @@ public void robotInit() { shooter.runFlywheelsCmd(() -> flywheelIdleSpeed, () -> flywheelIdleSpeed)); leds.setDefaultCommand( leds.defaultStateDisplayCmd( - () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER, () -> TalonFXFaultManager.getInstance().isOk())); + () -> DriverStation.isEnabled(), + () -> currentTarget == Target.SPEAKER, + () -> TalonFXFaultManager.getInstance().isOk())); controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java index d721cb99..bed9f157 100644 --- a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java +++ b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java @@ -8,7 +8,6 @@ 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 */ diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index df1257e2..1666130e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.utils.logging.TalonFXFaultManager; - import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java index c3eeafe1..fa4032f0 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java @@ -7,7 +7,6 @@ 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 */ diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 513c4d74..32e07bf3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -7,7 +7,6 @@ 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 */ diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java index 5114586a..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, BooleanSupplier faultsAreOk) { + public Command defaultStateDisplayCmd( + BooleanSupplier enabled, BooleanSupplier targetIsSpeaker, BooleanSupplier faultsAreOk) { return Commands.either( Commands.either( this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 8a1884e0..4786c6c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -20,7 +20,6 @@ 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; diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index cf5b596e..21a52cee 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -19,7 +19,6 @@ 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; @@ -62,8 +61,10 @@ public void updateInputs(final List asyncOdometrySamples) { public void periodic() { Logger.processInputs(String.format("Swerve/%s Module", io.getModuleName()), inputs); - 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); + 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 diff --git a/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java b/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java index 3f17340a..3b845b61 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXFaultManager.java @@ -4,58 +4,56 @@ 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; -import frc.robot.utils.logging.TalonFXLogger.TalonFXLog; - /** 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; + 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; + } } From c2d694eda51df9c83cd9ce7bf821ad61540e61eb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 2 Apr 2024 21:41:03 -0700 Subject: [PATCH 25/27] fix logger migration for accel ff --- src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java | 2 +- src/main/java/frc/robot/subsystems/vision/VisionIOReal.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index 77c37ab8..b664c500 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -214,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/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. ***/ From 8d7ee6efb2e4aa7ece16b860c082e90d86a82b84 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 3 Apr 2024 09:18:51 -0700 Subject: [PATCH 26/27] add statussignal errorcode to talonfxlog --- .../robot/utils/logging/TalonFXLogger.java | 40 ++++++++++++++++--- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index 8e45075f..5d4ca652 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -21,6 +21,9 @@ public static class TalonFXLog implements StructSerializable { public double position = 0.0; public double velocity = 0.0; + // StatusCode error value. Currently just checks one signal. + public int errorCode = 0; + // Faults public boolean licenseFault = false; @@ -31,6 +34,7 @@ public TalonFXLog( double temperatureCelsius, double position, double velocityRotationsPerSecond, + int errorCode, boolean licenseFault) { this.appliedVolts = appliedVolts; this.statorCurrentAmps = statorCurrentAmps; @@ -39,7 +43,28 @@ public TalonFXLog( this.position = position; this.velocity = velocityRotationsPerSecond; - licenseFault = false; + this.errorCode = errorCode; + + this.licenseFault = licenseFault; + } + + public TalonFXLog( + double appliedVolts, + double statorCurrentAmps, + double supplyCurrentAmps, + double temperatureCelsius, + double position, + double velocityRotationsPerSecond, + int errorCode) { + this( + appliedVolts, + statorCurrentAmps, + supplyCurrentAmps, + temperatureCelsius, + position, + velocityRotationsPerSecond, + errorCode, + false); } public TalonFXLog( @@ -56,6 +81,7 @@ public TalonFXLog( temperatureCelsius, position, velocityRotationsPerSecond, + 0, false); } @@ -75,12 +101,12 @@ public String getTypeString() { @Override public int getSize() { - return kSizeDouble * 6 + kSizeBool * 1; + return kSizeDouble * 6 + kSizeInt32 + kSizeBool * 1; } @Override public String getSchema() { - return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity;boolean licenseFault"; + return "double voltage;double statorAmps;double supplyAmps;double temp;double position;double velocity;int errorCode;boolean licenseFault"; } @Override @@ -91,9 +117,10 @@ public TalonFXLog unpack(ByteBuffer bb) { double temp = bb.getDouble(); double rotation = bb.getDouble(); double velocity = bb.getDouble(); + int errorCode = bb.getInt(); boolean licenseFault = bb.get() != 0; return new TalonFXLog( - voltage, statorAmps, supplyAmps, temp, rotation, velocity, licenseFault); + voltage, statorAmps, supplyAmps, temp, rotation, velocity, errorCode, licenseFault); } @Override @@ -109,7 +136,7 @@ public void pack(ByteBuffer bb, TalonFXLog value) { } } - public final TalonFXLog log = new TalonFXLog(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + public final TalonFXLog log = new TalonFXLog(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0); private final StatusSignal voltageSignal; private final StatusSignal statorCurrentSignal; @@ -164,6 +191,9 @@ public TalonFXLog update() { log.temperatureCelsius = temperatureSignal.getValueAsDouble(); log.position = positionSignal.getValueAsDouble(); log.velocity = velocitySignal.getValueAsDouble(); + + log.errorCode = voltageSignal.getStatus().value; + log.licenseFault = licenseFaultSignal.getValue(); return log; From a3fe0e00e4d64bd24785a902eed88a18e1e7d816 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 3 Apr 2024 09:30:03 -0700 Subject: [PATCH 27/27] store errorcode as statuscode --- .../frc/robot/utils/logging/TalonFXLogger.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java index 5d4ca652..0f418e6f 100644 --- a/src/main/java/frc/robot/utils/logging/TalonFXLogger.java +++ b/src/main/java/frc/robot/utils/logging/TalonFXLogger.java @@ -1,6 +1,7 @@ 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; @@ -22,7 +23,7 @@ public static class TalonFXLog implements StructSerializable { public double velocity = 0.0; // StatusCode error value. Currently just checks one signal. - public int errorCode = 0; + public StatusCode errorCode = StatusCode.StatusCodeNotInitialized; // Faults public boolean licenseFault = false; @@ -34,7 +35,7 @@ public TalonFXLog( double temperatureCelsius, double position, double velocityRotationsPerSecond, - int errorCode, + StatusCode errorCode, boolean licenseFault) { this.appliedVolts = appliedVolts; this.statorCurrentAmps = statorCurrentAmps; @@ -55,7 +56,7 @@ public TalonFXLog( double temperatureCelsius, double position, double velocityRotationsPerSecond, - int errorCode) { + StatusCode errorCode) { this( appliedVolts, statorCurrentAmps, @@ -81,7 +82,7 @@ public TalonFXLog( temperatureCelsius, position, velocityRotationsPerSecond, - 0, + StatusCode.StatusCodeNotInitialized, false); } @@ -117,7 +118,7 @@ public TalonFXLog unpack(ByteBuffer bb) { double temp = bb.getDouble(); double rotation = bb.getDouble(); double velocity = bb.getDouble(); - int errorCode = bb.getInt(); + var errorCode = StatusCode.valueOf(bb.getInt()); boolean licenseFault = bb.get() != 0; return new TalonFXLog( voltage, statorAmps, supplyAmps, temp, rotation, velocity, errorCode, licenseFault); @@ -136,7 +137,7 @@ public void pack(ByteBuffer bb, TalonFXLog value) { } } - public final TalonFXLog log = new TalonFXLog(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0); + 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; @@ -192,7 +193,7 @@ public TalonFXLog update() { log.position = positionSignal.getValueAsDouble(); log.velocity = velocitySignal.getValueAsDouble(); - log.errorCode = voltageSignal.getStatus().value; + log.errorCode = voltageSignal.getStatus(); log.licenseFault = licenseFaultSignal.getValue();