From 34a733b301959068daa27584a9b6ecfab629ff95 Mon Sep 17 00:00:00 2001 From: iefomit Date: Fri, 3 Apr 2026 00:10:59 -0700 Subject: [PATCH] boolean to disable logging --- src/main/java/frc/robot/constants/Constants.java | 3 +++ .../frc/robot/subsystems/Climb/LinearClimb.java | 10 +++++++--- .../java/frc/robot/subsystems/Intake/Intake.java | 12 ++++++++---- .../robot/subsystems/drivetrain/Drivetrain.java | 8 ++++++-- .../frc/robot/subsystems/drivetrain/Module.java | 13 ++++++++++--- src/main/java/frc/robot/subsystems/hood/Hood.java | 12 ++++++++---- .../frc/robot/subsystems/shooter/Shooter.java | 10 +++++++--- .../frc/robot/subsystems/spindexer/Spindexer.java | 10 +++++++--- .../java/frc/robot/subsystems/turret/Turret.java | 15 +++++++++++---- src/main/java/frc/robot/util/Vision/Vision.java | 5 ++++- 10 files changed, 71 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 2cf72f1..cfa252c 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -20,6 +20,9 @@ public class Constants { // Logging public static final boolean USE_TELEMETRY = true; + // this would disable all logger calls + public static final boolean DISABLE_LOGGING = true; + public static enum Mode { /** Running on a real robot. */ REAL, diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java index 14d97d3..f505917 100644 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java @@ -156,10 +156,14 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO { SmartDashboard.putBoolean("Climb Calibrated", !calibrating); SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint()); - Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) - * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) + * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO); + } updateInputs(); - Logger.processInputs("LinearClimb", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("LinearClimb", inputs); + } } /** diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index c1fca0a..488fef5 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -188,10 +188,12 @@ public class Intake extends SubsystemBase implements IntakeIO{ } public void periodic() { - // Report position to SmartDashboard double inchExtension = getPosition(); - Logger.recordOutput("Intake/Setpoint", setpointInches); - robotExtension.setLength(inchExtension); + + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Intake/Setpoint", setpointInches); + robotExtension.setLength(inchExtension); + } SmartDashboard.putNumber("Intake Extension (in)", inchExtension); SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0); @@ -206,7 +208,9 @@ public class Intake extends SubsystemBase implements IntakeIO{ } updateInputs(); - Logger.processInputs("Intake", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Intake", inputs); + } SmartDashboard.putBoolean("Intake Calibrated", !calibrating); SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 550765e..dc50b47 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -197,7 +197,9 @@ public class Drivetrain extends SubsystemBase { public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Drive/Gyro", gyroInputs); + } for (var module : modules) { module.periodic(); } @@ -228,7 +230,9 @@ public class Drivetrain extends SubsystemBase { // Apply update poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } - Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses()); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Odometry/module poses", modulePoses.getModulePoses()); + } updateOdometryVision(); field.setRobotPose(getPose()); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java index 7c01282..4ff8670 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -28,6 +28,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.Constants; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -209,7 +210,9 @@ public class Module implements ModuleIO{ public void periodic() { updateInputs(); - Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); + } // Calculate positions for odometry int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together @@ -223,7 +226,9 @@ public class Module implements ModuleIO{ driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); - Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); + } } public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { @@ -250,7 +255,9 @@ public class Module implements ModuleIO{ driveMotor.set(percentOutput); } else { double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO; - Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); + } double feedforward = velocity * moduleConstants.getDriveV(); driveMotor.setControl( diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 5620b76..34425fa 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -104,7 +104,9 @@ public class Hood extends SubsystemBase implements HoodIO { @Override public void periodic() { updateInputs(); - Logger.processInputs("Hood", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Hood", inputs); + } // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees())); // SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees()); @@ -151,9 +153,11 @@ public class Hood extends SubsystemBase implements HoodIO { .withFeedForward(velocityCompensation)); } - Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); - Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians())); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); + Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); + Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians())); + } SmartDashboard.putBoolean("Hood Calibrated", !calibrating); SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 74d1131..882ffc8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -93,8 +93,10 @@ public class Shooter extends SubsystemBase implements ShooterIO { shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS)); shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS)); - Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER); - Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER); + Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); + } double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; @@ -134,7 +136,9 @@ public class Shooter extends SubsystemBase implements ShooterIO { inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble(); - Logger.processInputs("Shooter", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Shooter", inputs); + } } /** diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index a4a1014..e61eafc 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -50,8 +50,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { @Override public void periodic() { - updateInputs(); - Logger.processInputs("Spindexer", inputs); + if (!Constants.DISABLE_LOGGING) { + updateInputs(); + Logger.processInputs("Spindexer", inputs); + } if (resetPos == null) { motor.setPosition(0.1 * gearRatio); @@ -133,7 +135,9 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { public void updateInputs() { inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio; inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble(); - Logger.processInputs("Spindexer", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Spindexer", inputs); + } } private Double resetPos; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 7217229..4762196 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.constants.Constants; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -159,7 +160,9 @@ public class Turret extends SubsystemBase implements TurretIO{ @Override public void periodic() { updateInputs(); - Logger.processInputs("Turret", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Turret", inputs); + } // Position extrapolation double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; @@ -216,14 +219,18 @@ public class Turret extends SubsystemBase implements TurretIO{ .withFeedForward(robotTurnCompensation)); } - Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees()); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); + Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees()); + } // --- Visualization --- ligament.setAngle(Units.radiansToDegrees(getPositionRad())); updateInputs(); - Logger.processInputs("Turret", inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Turret", inputs); + } SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad())); SmartDashboard.putBoolean("Turret Calibrated", !calibrating); diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java index ad53fa7..4b184cb 100644 --- a/src/main/java/frc/robot/util/Vision/Vision.java +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -32,6 +32,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; import frc.robot.constants.VisionConstants; import frc.robot.constants.swerve.DriveConstants; @@ -546,7 +547,9 @@ public class Vision { inputs.connected = camera.isConnected(); inputs.results = camera.getAllUnreadResults(); - Logger.processInputs("Vision/"+camera.getName(), inputs); + if (!Constants.DISABLE_LOGGING) { + Logger.processInputs("Vision/"+camera.getName(), inputs); + } // Mechanical Advantage's vision logging // // Read new camera observations -- 2.39.5