make it easy to enable/disable shuffleboard logging via utility wrapper

src/main/java/com/team766/logging/ShuffleboardUtil.java

@@ -0,0 +1,41 @@
+package com.team766.logging;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public final class ShuffleboardUtil {
+
+    private static boolean ENABLE_LOGGING = false;
+
+    // private static MatchType matchType = null;
+
+    private ShuffleboardUtil() {}
+
+    private static boolean shouldLog() {
+        return ENABLE_LOGGING;
+
+        // if (ENABLE_LOGGING_IF_NOT_COMP) {
+        //     return false;
+        // }
+
+        // TODO: test this more carefully when we have time.
+        // synchronized (matchType) {
+        //     if (matchType == MatchType.None) {
+        //         matchType = DriverStation.getMatchType();
+        //     }
+        // }
+
+        // return matchType != MatchType.None;
+    }
+
+    public static void putNumber(String key, double value) {
+        if (shouldLog()) SmartDashboard.putNumber(key, value);
+    }
+
+    public static void putBoolean(String key, boolean value) {
+        if (shouldLog()) SmartDashboard.putBoolean(key, value);
+    }
+
+    public static void putString(String key, String value) {
+        if (shouldLog()) SmartDashboard.putString(key, value);
+    }
+}

src/main/java/com/team766/odometry/Odometry.java

@@ -8,6 +8,7 @@
 import com.team766.logging.Category;
 import com.team766.logging.Logger;
 import com.team766.logging.Severity;
+import com.team766.logging.ShuffleboardUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
@@ -174,23 +175,23 @@ private void updateCurrentPositions() {
                                     .plus(new Transform2d(wheelPositions[i], new Rotation2d()))
                                     .getTranslation(),
                             currPositions[i].getRotation());
-            // SmartDashboard.putNumber(
-            //         "early curr rotation", currPositions[i].getRotation().getDegrees());
-            // SmartDashboard.putString("prev rotation direct", prevPositions[i].toString());
+            ShuffleboardUtil.putNumber(
+                    "early curr rotation", currPositions[i].getRotation().getDegrees());
+            ShuffleboardUtil.putString("prev rotation direct", prevPositions[i].toString());
             currPositions[i] =
                     new Pose2d(
                             currPositions[i].getTranslation(),
                             gyroPosition.plus(Rotation2d.fromDegrees(absolutePosition)));
 
             rotationChange = currPositions[i].getRotation().minus(prevPositions[i].getRotation());
-            // SmartDashboard.putNumber("curr rotation",
-            // currPositions[i].getRotation().getDegrees());
-            // SmartDashboard.putNumber("prev rotation",
-            // prevPositions[i].getRotation().getDegrees());
-            // SmartDashboard.putNumber("rotation change", rotationChange.getDegrees());
+            ShuffleboardUtil.putNumber(
+                    "curr rotation", currPositions[i].getRotation().getDegrees());
+            ShuffleboardUtil.putNumber(
+                    "prev rotation", prevPositions[i].getRotation().getDegrees());
+            ShuffleboardUtil.putNumber("rotation change", rotationChange.getDegrees());
 
             double yaw = Math.toRadians(gyro.getAngle());
-            // SmartDashboard.putNumber("odom yaw", yaw);
+            ShuffleboardUtil.putNumber("odom yaw", yaw);
             double roll = Math.toRadians(gyro.getRoll());
             double pitch = Math.toRadians(gyro.getPitch());
 

src/main/java/com/team766/robot/common/mechanisms/Drive.java

@@ -10,6 +10,7 @@
 import com.team766.hal.RobotProvider;
 import com.team766.logging.Category;
 import com.team766.logging.Logger;
+import com.team766.logging.ShuffleboardUtil;
 import com.team766.odometry.Odometry;
 import com.team766.robot.common.SwerveConfig;
 import com.team766.robot.common.constants.ConfigConstants;
@@ -163,8 +164,8 @@ private static Vector2D createOrthogonalVector(Vector2D vector) {
      */
     public void controlRobotOriented(double x, double y, double turn) {
         checkContextOwnership();
-        // SmartDashboard.putString(
-        //         "[" + "joystick" + "]" + "x, y", String.format("%.2f, %.2f", x, y));
+        ShuffleboardUtil.putString(
+                "[" + "joystick" + "]" + "x, y", String.format("%.2f, %.2f", x, y));
 
         // Calculate the necessary turn velocity (m/s) for each motor:
         double turnVelocity = config.wheelDistanceFromCenter() * turn;
@@ -239,7 +240,7 @@ public void controlFieldOrientedWithRotationTarget(double x, double y, Rotation2
         checkContextOwnership();
         if (target != null) {
             rotationPID.setSetpoint(target.getDegrees());
-            // SmartDashboard.putNumber("Rotation Target", target.getDegrees());
+            ShuffleboardUtil.putNumber("Rotation Target", target.getDegrees());
         }
 
         movingToTarget = true;
@@ -257,7 +258,7 @@ public void controlFieldOrientedWithRotationTarget(double x, double y, Rotation2
     public boolean isAtRotationTarget() {
         boolean value =
                 Math.abs(rotationPID.getOutput()) < ControlConstants.DEFAULT_ROTATION_THRESHOLD;
-        // SmartDashboard.putBoolean("Is At Drive Rotation Target", value);
+        // ShuffleboardUtil.putBoolean("Is At Drive Rotation Target", value);
         return value;
     }
 
@@ -384,11 +385,11 @@ private static Translation2d getPositionForWheel(
     public void run() {
         swerveOdometry.run();
         // log(currentPosition.toString());
-        // SmartDashboard.putString("pos", getCurrentPosition().toString());
+        ShuffleboardUtil.putString("pos", getCurrentPosition().toString());
 
-        // SmartDashboard.putNumber("Yaw", getHeading());
-        // SmartDashboard.putNumber("Pitch", getPitch());
-        // SmartDashboard.putNumber("Roll", getRoll());
+        ShuffleboardUtil.putNumber("Yaw", getHeading());
+        ShuffleboardUtil.putNumber("Pitch", getPitch());
+        ShuffleboardUtil.putNumber("Roll", getRoll());
 
         if (movingToTarget) {
             rotationPID.calculate(getHeading());
@@ -400,9 +401,9 @@ public void run() {
                             : rotationPID.getOutput()));
         }
 
-        // SmartDashboard.putBoolean("movingToTarget", movingToTarget);
+        ShuffleboardUtil.putBoolean("movingToTarget", movingToTarget);
 
-        // SmartDashboard.putBoolean("isAtRotationTarget", isAtRotationTarget());
+        ShuffleboardUtil.putBoolean("isAtRotationTarget", isAtRotationTarget());
 
         swerveFR.dashboardCurrentUsage();
         swerveFL.dashboardCurrentUsage();

src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java

@@ -7,10 +7,10 @@
 import com.team766.logging.Category;
 import com.team766.logging.Logger;
 import com.team766.logging.Severity;
+import com.team766.logging.ShuffleboardUtil;
 import com.team766.robot.reva.mechanisms.MotorUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
 
 /**
@@ -77,7 +77,7 @@ public SwerveModule(
         this.steer = steer;
         this.encoder = encoder;
         this.offset = computeEncoderOffset();
-        // SmartDashboard.putNumber("[" + modulePlacement + "]" + "Offset", offset);
+        ShuffleboardUtil.putNumber("[" + modulePlacement + "]" + "Offset", offset);
 
         // Current limit for motors to avoid breaker problems
         drive.setCurrentLimit(driveMotorCurrentLimit);
@@ -109,9 +109,9 @@ private double computeEncoderOffset() {
      */
     public void steer(Vector2D vector) {
         boolean reversed = false;
-        // SmartDashboard.putString(
-        //         "[" + modulePlacement + "]" + "x, y",
-        //         String.format("%.2f, %.2f", vector.getX(), vector.getY()));
+        ShuffleboardUtil.putString(
+                "[" + modulePlacement + "]" + "x, y",
+                String.format("%.2f, %.2f", vector.getX(), vector.getY()));
 
         // Calculates the angle of the vector from -180° to 180°
         final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX()));
@@ -143,16 +143,16 @@ public void steer(Vector2D vector) {
         // Sets the degree of the steer wheel
         // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor
         // understands
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + "Steer", ENCODER_CONVERSION_FACTOR * angleDegrees);
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + "Steer", ENCODER_CONVERSION_FACTOR * angleDegrees);
 
         steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees);
 
-        // SmartDashboard.putNumber("[" + modulePlacement + "]" + "TargetAngle", vectorTheta);
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + "RelativeAngle",
-        //         steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR - offset);
-        SmartDashboard.putNumber(
+        ShuffleboardUtil.putNumber("[" + modulePlacement + "]" + "TargetAngle", vectorTheta);
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + "RelativeAngle",
+                steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR - offset);
+        ShuffleboardUtil.putNumber(
                 "[" + modulePlacement + "]" + "CANCoder",
                 encoder.getAbsolutePosition().getValueAsDouble() * 360);
         // return reversed;
@@ -168,8 +168,7 @@ public void driveAndSteer(Vector2D vector) {
 
         // sets the power to the magnitude of the vector and reverses power if necessary
         // TODO: does this need to be clamped to a specific range, eg btn -1 and 1?
-        // SmartDashboard.putNumber("[" + modulePlacement + "]" + "Desired drive",
-        // vector.getNorm());
+        ShuffleboardUtil.putNumber("[" + modulePlacement + "]" + "Desired drive", vector.getNorm());
         double power;
         // if (reversed) {
         //    power = -vector.getNorm() * MOTOR_WHEEL_FACTOR_MPS;
@@ -178,11 +177,11 @@ public void driveAndSteer(Vector2D vector) {
         // } else {
         power = vector.getNorm() * MOTOR_WHEEL_FACTOR_MPS;
         // }
-        SmartDashboard.putNumber("[" + modulePlacement + "]" + "Input motor velocity", power);
+        ShuffleboardUtil.putNumber("[" + modulePlacement + "]" + "Input motor velocity", power);
         drive.set(ControlMode.Velocity, power);
 
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + "Read Vel", drive.getSensorVelocity());
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + "Read Vel", drive.getSensorVelocity());
     }
 
     /**
@@ -200,17 +199,17 @@ public SwerveModuleState getModuleState() {
     }
 
     public void dashboardCurrentUsage() {
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + " steer supply current",
-        //         MotorUtil.getCurrentUsage(steer));
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + " steer stator current",
-        //         MotorUtil.getStatorCurrentUsage(steer));
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + " drive supply current",
-        //         MotorUtil.getCurrentUsage(drive));
-        // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + " drive stator current",
-        //         MotorUtil.getStatorCurrentUsage(drive));
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + " steer supply current",
+                MotorUtil.getCurrentUsage(steer));
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + " steer stator current",
+                MotorUtil.getStatorCurrentUsage(steer));
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + " drive supply current",
+                MotorUtil.getCurrentUsage(drive));
+        ShuffleboardUtil.putNumber(
+                "[" + modulePlacement + "]" + " drive stator current",
+                MotorUtil.getStatorCurrentUsage(drive));
     }
 }

src/main/java/com/team766/robot/gatorade/OI.java

@@ -7,13 +7,13 @@
 import com.team766.library.RateLimiter;
 import com.team766.logging.Category;
 import com.team766.logging.Severity;
+import com.team766.logging.ShuffleboardUtil;
 import com.team766.robot.common.DriverOI;
 import com.team766.robot.gatorade.constants.ControlConstants;
 import com.team766.robot.gatorade.constants.InputConstants;
 import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType;
 import com.team766.robot.gatorade.procedures.*;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -56,7 +56,7 @@ public void run(Context context) {
             context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
             RobotProvider.instance.refreshDriverStationData();
 
-            SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());
+            ShuffleboardUtil.putString("Alliance", DriverStation.getAlliance().toString());
 
             // Add driver controls here - make sure to take/release ownership
             // of mechanisms when appropriate.
@@ -76,11 +76,11 @@ public void run(Context context) {
             if (boxopGamepad.getPOV() == InputConstants.POV_UP) {
                 new GoForCones().run(context);
                 setLightsForGamePiece();
-                SmartDashboard.putBoolean("Game Piece", true);
+                ShuffleboardUtil.putBoolean("Game Piece", true);
             } else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) {
                 new GoForCubes().run(context);
                 setLightsForGamePiece();
-                SmartDashboard.putBoolean("Game Piece", false);
+                ShuffleboardUtil.putBoolean("Game Piece", false);
             }
 
             // look for button presses to queue placement of intake/wrist/elevator superstructure

src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java

@@ -13,8 +13,8 @@
 import com.team766.library.RateLimiter;
 import com.team766.library.ValueProvider;
 import com.team766.logging.Severity;
+import com.team766.logging.ShuffleboardUtil;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * Basic elevator mechanism.  Used in conjunction with the {@link Intake} and {@link Wrist}.
@@ -185,8 +185,8 @@ public void moveTo(double position) {
         // convert the desired target degrees to encoder units
         double rotations = EncoderUtils.elevatorHeightToRotations(position);
 
-        // SmartDashboard.putNumber("[ELEVATOR] ff", ff);
-        SmartDashboard.putNumber("[ELEVATOR] reference", rotations);
+        // ShuffleboardUtil.putNumber("[ELEVATOR] ff", ff);
+        ShuffleboardUtil.putNumber("[ELEVATOR] reference", rotations);
 
         // set the reference point for the wrist
         pidController.setReference(rotations, ControlType.kPosition, 0, ff);
@@ -195,8 +195,8 @@ public void moveTo(double position) {
     @Override
     public void run() {
         if (rateLimiter.next()) {
-            SmartDashboard.putNumber("[ELEVATOR] Height", getHeight());
-            SmartDashboard.putNumber("[ELEVATOR] Rotations", getRotations());
+            ShuffleboardUtil.putNumber("[ELEVATOR] Height", getHeight());
+            ShuffleboardUtil.putNumber("[ELEVATOR] Rotations", getRotations());
         }
     }
 }

src/main/java/com/team766/robot/reva/OI.java

@@ -46,9 +46,6 @@ public void run(Context context) {
             context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
             RobotProvider.instance.refreshDriverStationData();
 
-            // NOTE: DriverStation.getAlliance() returns Optional<Alliance>
-            // SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());
-
             // Add driver controls here - make sure to take/release ownership
             // of mechanisms when appropriate.
 

src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java

@@ -2,6 +2,7 @@
 
 import com.team766.ViSIONbase.AprilTagGeneralCheckedException;
 import com.team766.ViSIONbase.GrayScaleCamera;
+import com.team766.logging.ShuffleboardUtil;
 import com.team766.robot.common.mechanisms.Drive;
 import com.team766.robot.reva.Robot;
 import com.team766.robot.reva.constants.VisionConstants;
@@ -68,7 +69,7 @@ private boolean updateTarget() {
                                     relativeTarget.rotateBy(
                                             Rotation2d.fromDegrees((drive.getHeading() + 180))));
 
-            // SmartDashboard.putString("target pos", absTargetPos.toString());
+            ShuffleboardUtil.putString("target pos", absTargetPos.toString());
 
             // context.takeOwnership(drive);
 
@@ -113,8 +114,8 @@ public void update() {
         updateAlliance();
         updateTarget();
         updateRelativeTranslation2d();
-        // SmartDashboard.putString("translation", relativeTranslation2d.toString());
-        // SmartDashboard.putNumber("Tag Dist", relativeTranslation2d.getNorm());
+        ShuffleboardUtil.putString("translation", relativeTranslation2d.toString());
+        ShuffleboardUtil.putNumber("Tag Dist", relativeTranslation2d.getNorm());
     }
 
     public Rotation2d getHeadingToTarget() {
@@ -125,23 +126,23 @@ public Rotation2d getHeadingToTarget() {
         // Calculated the heading the robot needs to face from this translation
 
         double val = relativeTranslation2d.getAngle().getDegrees() + drive.getHeading();
-        // SmartDashboard.putNumber(
-        //         "relativeTranslation2d angle", relativeTranslation2d.getAngle().getDegrees());
-        // SmartDashboard.putNumber(
-        //         "heading angle", Rotation2d.fromDegrees(drive.getHeading()).getDegrees());
-        // SmartDashboard.putNumber("output heading", val);
+        ShuffleboardUtil.putNumber(
+                "relativeTranslation2d angle", relativeTranslation2d.getAngle().getDegrees());
+        ShuffleboardUtil.putNumber(
+                "heading angle", Rotation2d.fromDegrees(drive.getHeading()).getDegrees());
+        ShuffleboardUtil.putNumber("output heading", val);
         return Rotation2d.fromDegrees(val);
     }
 
     public double getShooterPower() throws AprilTagGeneralCheckedException {
         double val = VisionPIDProcedure.getBestPowerToUse(relativeTranslation2d.getNorm());
-        // SmartDashboard.putNumber("shooter power", val);
+        ShuffleboardUtil.putNumber("shooter power", val);
         return val;
     }
 
     public double getArmAngle() throws AprilTagGeneralCheckedException {
         double val = VisionPIDProcedure.getBestArmAngleToUse(relativeTranslation2d.getNorm());
-        // SmartDashboard.putNumber("arm angle", val);
+        ShuffleboardUtil.putNumber("arm angle", val);
         return val;
     }
 }