From a8004c868fabebcc00ab4e640d845e5beab8ce5b Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Fri, 3 Apr 2026 07:50:40 -0700 Subject: [PATCH] all smart dashboard enable/disable besides autopicker --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++---- .../drive_comm/DefaultDriveCommand.java | 6 ++- .../robot/commands/gpm/HardstopWarning.java | 7 +++- .../frc/robot/commands/gpm/RunSpindexer.java | 5 ++- .../robot/commands/gpm/Superstructure.java | 40 ++++++++++++------- .../commands/vision/TestVisionDistance.java | 12 ++++-- .../java/frc/robot/constants/Constants.java | 1 + .../robot/subsystems/Climb/LinearClimb.java | 26 +++++++----- .../frc/robot/subsystems/Intake/Intake.java | 25 +++++++----- .../subsystems/drivetrain/Drivetrain.java | 5 ++- .../java/frc/robot/subsystems/hood/Hood.java | 23 ++++++----- .../frc/robot/subsystems/shooter/Shooter.java | 30 ++++++++------ .../robot/subsystems/spindexer/Spindexer.java | 20 ++++++---- .../frc/robot/subsystems/turret/Turret.java | 23 +++++++---- 14 files changed, 157 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a1d15bc..dd147dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,9 +102,11 @@ public class RobotContainer { */ public RobotContainer(RobotId robotId) { // display the current robot id on smartdashboard - SmartDashboard.putString("RobotID", robotId.toString()); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putString("RobotID", robotId.toString()); - SmartDashboard.putNumber("Match Time", 0.0); + SmartDashboard.putNumber("Match Time", 0.0); + } // Filling the SendableChooser on SmartDashboard @@ -190,7 +192,9 @@ public class RobotContainer { LiveWindow.disableAllTelemetry(); LiveWindow.setEnabled(false); - SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis()); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis()); + } } /** @@ -404,12 +408,18 @@ public class RobotContainer { double elapsed = matchTimer.get(); countdownTime = Math.max(0, currentPhaseDuration - elapsed); } - SmartDashboard.putNumber("Phase Countdown", countdownTime); - SmartDashboard.putString("Current Phase", currentPhase); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Phase Countdown", countdownTime); + SmartDashboard.putString("Current Phase", currentPhase); + } if (matchTime > 0) { - SmartDashboard.putNumber("Match Time", matchTime); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Match Time", matchTime); + } + } + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); } - SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); } } diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index a99c5c6..1e5d23a 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -42,7 +42,9 @@ public class DefaultDriveCommand extends Command { trenchAssistPid.setIZone(2); trenchAssistPid.setIntegratorRange(-1, 1); - SmartDashboard.putNumber("0 degrees snap location", 0); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("0 degrees snap location", 0); + } } @Override @@ -96,7 +98,7 @@ public class DefaultDriveCommand extends Command { if (!Constants.DISABLE_LOGGING) { Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist()); } - + if (swerve.getTrenchAssist()) { drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid)); } else { diff --git a/src/main/java/frc/robot/commands/gpm/HardstopWarning.java b/src/main/java/frc/robot/commands/gpm/HardstopWarning.java index 4f0107f..a1fd2f1 100644 --- a/src/main/java/frc/robot/commands/gpm/HardstopWarning.java +++ b/src/main/java/frc/robot/commands/gpm/HardstopWarning.java @@ -2,6 +2,7 @@ package frc.robot.commands.gpm; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; import frc.robot.constants.IntakeConstants; import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.hood.Hood; @@ -24,8 +25,10 @@ public class HardstopWarning extends Command { @Override public void execute() { double epsilon = 0.05; - SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() >= HoodConstants.MIN_ANGLE - epsilon); - SmartDashboard.putBoolean("Intake OK", intake.getPosition() >= IntakeConstants.STARTING_POINT - epsilon); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() >= HoodConstants.MIN_ANGLE - epsilon); + SmartDashboard.putBoolean("Intake OK", intake.getPosition() >= IntakeConstants.STARTING_POINT - epsilon); + } } @Override diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java index d73cfe5..4a6ac7c 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.spindexer.SpindexerConstants; @@ -68,7 +69,9 @@ public class RunSpindexer extends Command { reversing = false; } } - SmartDashboard.putBoolean("Spindexer Jamming", reversing); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Spindexer Jamming", reversing); + } } @Override diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index c2a34d1..02ca914 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -152,9 +152,11 @@ public class Superstructure extends Command { Logger.recordOutput("Turret/Target Pose", target); Logger.recordOutput("Lookahead Pose", lookaheadPose); } - SmartDashboard.putNumber("Time of flight", timeOfFlight); - SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); - SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Time of flight", timeOfFlight); + SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); + SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); + } // Subtract the rotational angle of the robot from the setpoint double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); @@ -231,12 +233,19 @@ public class Superstructure extends Command { @Override public void execute() { - TOFAdjustment = SmartDashboard.getNumber("OPERATOR: TOF Adjustment", TOFAdjustment); - SmartDashboard.putNumber("OPERATOR: TOF Adjustment", TOFAdjustment); - hoodOffset = SmartDashboard.getNumber("OPERATOR: Hood Offset", hoodOffset); - SmartDashboard.putNumber("OPERATOR: Hood Offset", hoodOffset); - turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset); - SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset); + if (!Constants.DISABLE_SMART_DASHBOARD) { + TOFAdjustment = SmartDashboard.getNumber("OPERATOR: TOF Adjustment", TOFAdjustment); + SmartDashboard.putNumber("OPERATOR: TOF Adjustment", TOFAdjustment); + hoodOffset = SmartDashboard.getNumber("OPERATOR: Hood Offset", hoodOffset); + SmartDashboard.putNumber("OPERATOR: Hood Offset", hoodOffset); + turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset); + SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset); + } else { + TOFAdjustment = 0.0; + hoodOffset = 0.0; + turretOffset = 0.0; + } + // Phase manager stuff phaseManager.update(drivepose, shooter, turret); target = phaseManager.getTarget(drivepose); @@ -289,13 +298,16 @@ public class Superstructure extends Command { Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation())); } - - - SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); // for operator - phaseDelay = SmartDashboard.getNumber("OPERATOR: Phase Delay", phaseDelay); - SmartDashboard.putNumber("OPERATOR: Phase Delay", phaseDelay); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); + + phaseDelay = SmartDashboard.getNumber("OPERATOR: Phase Delay", phaseDelay); + SmartDashboard.putNumber("OPERATOR: Phase Delay", phaseDelay); + } else { + phaseDelay = 0.03; + } } @Override diff --git a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java index 91e6c40..f974005 100644 --- a/src/main/java/frc/robot/commands/vision/TestVisionDistance.java +++ b/src/main/java/frc/robot/commands/vision/TestVisionDistance.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.util.Vision.Vision; @@ -77,10 +78,13 @@ public class TestVisionDistance extends Command { endTimer.reset(); driveDistance = drive.getPose().getTranslation().getDistance(driveStartTranslation); visionDistance = currentPose.getTranslation().getDistance(visionStartTranslation); - SmartDashboard.putNumber("Vision test drive distance", driveDistance); - SmartDashboard.putNumber("Vision test vision distnace", visionDistance); - SmartDashboard.putNumber("Vision test error", visionDistance - driveDistance); - SmartDashboard.putNumber("Vision test % error", (visionDistance-driveDistance) / driveDistance * 100); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Vision test drive distance", driveDistance); + SmartDashboard.putNumber("Vision test vision distnace", visionDistance); + SmartDashboard.putNumber("Vision test error", visionDistance - driveDistance); + SmartDashboard.putNumber("Vision test % error", (visionDistance-driveDistance) / driveDistance * 100); + } + // If kPrintDelay seconds have passed, print the data if (printTimer.advanceIfElapsed(PRINT_DELAY)) { System.out.printf("\nDrive dist: %.2f\nVision dist: %.2f\nError: %.2f\n %% error: %.2f\n", diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 3c1e2dd..50d0530 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -22,6 +22,7 @@ public class Constants { // this would disable all logger calls public static final boolean DISABLE_LOGGING = false; + public static final boolean DISABLE_SMART_DASHBOARD = false; // wont disable auto picker public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java index f505917..0e2aa31 100644 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java @@ -56,10 +56,12 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO { setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT); - SmartDashboard.putData("Calibrate", new InstantCommand(() -> hardstopCalibration())); - SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); // 0 - SmartDashboard.putData("Go Down", new InstantCommand(() -> retract())); // 8 - SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition())); // 6 + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Calibrate", new InstantCommand(() -> hardstopCalibration())); + SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); // 0 + SmartDashboard.putData("Go Down", new InstantCommand(() -> retract())); // 8 + SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition())); // 6 + } // starting position motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION)); @@ -148,13 +150,15 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO { } } - // motor.set(power); // during calibration we have 20ms of high power before we stop calibration - SmartDashboard.putNumber("Climb Power from PID", power); - SmartDashboard.putNumber("Climb PID_Setpoint_Rotations", pid.getSetpoint()); - SmartDashboard.putNumber("Climb Motor_Actual_Rotations", motor.getPosition().getValueAsDouble()); - SmartDashboard.putNumber("Climb Motor_Actual_Meters", inputs.positionMeters); - SmartDashboard.putBoolean("Climb Calibrated", !calibrating); - SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint()); + // motor.set(power); // during calibration we have 20ms of high power before we stop calibration\ + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Climb Power from PID", power); + SmartDashboard.putNumber("Climb PID_Setpoint_Rotations", pid.getSetpoint()); + SmartDashboard.putNumber("Climb Motor_Actual_Rotations", motor.getPosition().getValueAsDouble()); + SmartDashboard.putNumber("Climb Motor_Actual_Meters", inputs.positionMeters); + SmartDashboard.putBoolean("Climb Calibrated", !calibrating); + SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint()); + } if (!Constants.DISABLE_LOGGING) { Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 488fef5..340394d 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -153,11 +153,13 @@ public class Intake extends SubsystemBase implements IntakeIO{ robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 0, 90, 2, new Color8Bit(255, 0, 0) )); // add some test commands. - SmartDashboard.putData("Extension Mechanism", mechanism); - SmartDashboard.putData("Intake Calibrate", new InstantCommand(() -> calibrate())); - SmartDashboard.putData("Intake Stop Calibrating", new InstantCommand(() -> stopCalibrating())); - SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend())); - SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract())); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Extension Mechanism", mechanism); + SmartDashboard.putData("Intake Calibrate", new InstantCommand(() -> calibrate())); + SmartDashboard.putData("Intake Stop Calibrating", new InstantCommand(() -> stopCalibrating())); + SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend())); + SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract())); + } if (RobotBase.isSimulation()) { // Extender simulation @@ -194,9 +196,10 @@ public class Intake extends SubsystemBase implements IntakeIO{ Logger.recordOutput("Intake/Setpoint", setpointInches); robotExtension.setLength(inchExtension); } - - SmartDashboard.putNumber("Intake Extension (in)", inchExtension); - SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Intake Extension (in)", inchExtension); + SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0); + } if(calibrating){ leftMotor.set(-0.1); @@ -212,8 +215,10 @@ public class Intake extends SubsystemBase implements IntakeIO{ Logger.processInputs("Intake", inputs); } - SmartDashboard.putBoolean("Intake Calibrated", !calibrating); - SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Intake Calibrated", !calibrating); + SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5); + } } public void simulationPeriodic(){ diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 4415c8a..bb54ac8 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -186,8 +186,9 @@ public class Drivetrain extends SubsystemBase { }); // PPLibTelemetry.enableCompetitionMode(); - - SmartDashboard.putData("Field", field); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Field", field); + } } public void close() { diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 34425fa..bbc3010 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -61,12 +61,14 @@ public class Hood extends SubsystemBase implements HoodIO { motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); - SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); - SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); - SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); - - SmartDashboard.putData("force hood down", new InstantCommand(() -> forceHoodDown(true))); - SmartDashboard.putData("unforce hood", new InstantCommand(() -> forceHoodDown(false))); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); + SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); + SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); + + SmartDashboard.putData("force hood down", new InstantCommand(() -> forceHoodDown(true))); + SmartDashboard.putData("unforce hood", new InstantCommand(() -> forceHoodDown(false))); + } } /** @@ -158,10 +160,11 @@ public class Hood extends SubsystemBase implements HoodIO { 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); - + + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Hood Calibrated", !calibrating); + SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0); + } } public void calibrate(){ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0e21a81..376de5d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -68,8 +68,10 @@ public class Shooter extends SubsystemBase implements ShooterIO { shooterMotorLeft.getConfigurator().apply(limitConfig); shooterMotorRight.getConfigurator().apply(limitConfig); - SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); - SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0))); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); + SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0))); + } } @Override @@ -79,15 +81,18 @@ public class Shooter extends SubsystemBase implements ShooterIO { // shooterTargetSpeed = SmartDashboard.getNumber("Shooter Setpoint", shooterTargetSpeed); // SmartDashboard.putNumber("Shooter Setpoint", shooterTargetSpeed); - powerModifier = SmartDashboard.getNumber("OPERATOR: Shooter Power Modifier", powerModifier); - SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); + if (!Constants.DISABLE_SMART_DASHBOARD) { // yes I could put this in one, but more lines for me + powerModifier = SmartDashboard.getNumber("OPERATOR: Shooter Power Modifier", powerModifier); + SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); + } // Convert to RPS double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)) * powerModifier; - - SmartDashboard.putNumber("Target Velocity RPS", targetVelocityRPS); - SmartDashboard.putNumber("Shooter Motor RPS", shooterMotorLeft.getVelocity().getValueAsDouble()); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Target Velocity RPS", targetVelocityRPS); + SmartDashboard.putNumber("Shooter Motor RPS", shooterMotorLeft.getVelocity().getValueAsDouble()); + } // Sets the motor control to target velocity shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS)); @@ -100,11 +105,12 @@ public class Shooter extends SubsystemBase implements ShooterIO { double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; - SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity); - SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); - SmartDashboard.putBoolean("Shooter At Speed", atTargetSpeed()); - SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0); - + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity); + SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); + SmartDashboard.putBoolean("Shooter At Speed", atTargetSpeed()); + SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0); + } } /** diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index b24f30e..9825fb8 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -33,9 +33,11 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { limitConfig.SupplyCurrentLowerTime = 1.5; motor.getConfigurator().apply(limitConfig); - SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer())); - SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer())); - SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer())); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer())); + SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer())); + SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer())); + } resetPID.setTolerance(0.05); } @@ -79,10 +81,12 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { // scale threshold based on power double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power; - SmartDashboard.putNumber("Spindexer Ball Count", ballCount); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Spindexer Ball Count", ballCount); - SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM); - SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0); + SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM); + SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0); + } boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold; if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) { @@ -90,7 +94,9 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { } wasSpindexerSlow = isSpindexerSlow; - SmartDashboard.putBoolean("Spindexer Jamming", state == SpindexerState.REVERSE); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Spindexer Jamming", state == SpindexerState.REVERSE); + } } public void maxSpindexer() { diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 82a2502..ebc3f65 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -100,10 +100,12 @@ public class Turret extends SubsystemBase implements TurretIO{ 0.0); } - SmartDashboard.putData("Turret Mech", mech); - SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); - SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); - SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition())); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Turret Mech", mech); + SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); + SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); + SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition())); + } SendableChooser turretTestChooser = new SendableChooser<>(); turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); @@ -111,7 +113,10 @@ public class Turret extends SubsystemBase implements TurretIO{ turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0))); turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); - SmartDashboard.putData("Turret Test Positions", turretTestChooser); + + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Turret Test Positions", turretTestChooser); + } //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); motor.setPosition(0.0); @@ -231,9 +236,11 @@ public class Turret extends SubsystemBase implements TurretIO{ Logger.processInputs("Turret", inputs); } - SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad())); - SmartDashboard.putBoolean("Turret Calibrated", !calibrating); - SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint()); + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad())); + SmartDashboard.putBoolean("Turret Calibrated", !calibrating); + SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint()); + } } /* ---------------- Simulation ---------------- */ -- 2.39.5