From 9ff9ff31ac30752e68ccbd74049c452ee8808299 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Sun, 24 Mar 2024 14:31:18 -0700 Subject: [PATCH 1/8] create simple helper that lets us avoid logging to Shuffleboard if we're in a comp match --- .../com/team766/logging/ShuffleboardUtil.java | 40 +++++++++++++ .../java/com/team766/odometry/Odometry.java | 19 ++++--- .../robot/common/mechanisms/Drive.java | 21 +++---- .../robot/common/mechanisms/SwerveModule.java | 57 +++++++++---------- .../java/com/team766/robot/gatorade/OI.java | 8 +-- .../robot/gatorade/mechanisms/Elevator.java | 10 ++-- src/main/java/com/team766/robot/reva/OI.java | 3 - .../reva/VisionUtil/VisionSpeakerHelper.java | 21 +++---- .../robot/reva/mechanisms/Climber.java | 32 +++++------ .../mechanisms/ForwardApriltagCamera.java | 7 ++- .../team766/robot/reva/mechanisms/Intake.java | 9 +-- .../robot/reva/mechanisms/Shooter.java | 17 +++--- .../robot/reva/mechanisms/Shoulder.java | 43 +++++++------- 13 files changed, 164 insertions(+), 123 deletions(-) create mode 100644 src/main/java/com/team766/logging/ShuffleboardUtil.java diff --git a/src/main/java/com/team766/logging/ShuffleboardUtil.java b/src/main/java/com/team766/logging/ShuffleboardUtil.java new file mode 100644 index 000000000..bf43bdcea --- /dev/null +++ b/src/main/java/com/team766/logging/ShuffleboardUtil.java @@ -0,0 +1,40 @@ +package com.team766.logging; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.MatchType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public final class ShuffleboardUtil { + + private static boolean ENABLE_LOGGING_IF_NOT_COMP = true; + private static MatchType matchType = null; + + private ShuffleboardUtil() {} + + private static boolean shouldLog() { + + if (ENABLE_LOGGING_IF_NOT_COMP) { + return false; + } + + 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); + } +} diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java index f500a7c73..05f24e837 100644 --- a/src/main/java/com/team766/odometry/Odometry.java +++ b/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()); diff --git a/src/main/java/com/team766/robot/common/mechanisms/Drive.java b/src/main/java/com/team766/robot/common/mechanisms/Drive.java index 761e7edc8..8653dfa77 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/Drive.java +++ b/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(); diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java index a777f9d48..b41476589 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java +++ b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java @@ -10,7 +10,7 @@ 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 edu.wpi.first.wpilibj.ShuffleboardUtil.ShuffleboardUtil; 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)); } } diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index 7e7124bc7..f15b08d60 100644 --- a/src/main/java/com/team766/robot/gatorade/OI.java +++ b/src/main/java/com/team766/robot/gatorade/OI.java @@ -13,7 +13,7 @@ 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; +import edu.wpi.first.wpilibj.ShuffleboardUtil.ShuffleboardUtil; /** * 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 diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java index 1776a92bc..475bbf6cb 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Elevator.java +++ b/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()); } } } diff --git a/src/main/java/com/team766/robot/reva/OI.java b/src/main/java/com/team766/robot/reva/OI.java index 32d637782..b2e85882f 100644 --- a/src/main/java/com/team766/robot/reva/OI.java +++ b/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 - // SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); - // Add driver controls here - make sure to take/release ownership // of mechanisms when appropriate. diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java b/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java index 2768bfa69..b23c463c1 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java +++ b/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; } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Climber.java b/src/main/java/com/team766/robot/reva/mechanisms/Climber.java index 998352d1a..68bb7f6b4 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Climber.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Climber.java @@ -6,6 +6,7 @@ import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; +import com.team766.logging.ShuffleboardUtil; public class Climber extends Mechanism { @@ -131,21 +132,20 @@ public boolean isRightAtBottom() { @Override public void run() { - // SmartDashboard.putNumber("[CLIMBER] Left Rotations", leftMotor.getSensorPosition()); - // SmartDashboard.putNumber("[CLIMBER] Right Rotations", rightMotor.getSensorPosition()); - // SmartDashboard.putNumber("[CLIMBER] Left Height", getHeightLeft()); - // SmartDashboard.putNumber("[CLIMBER] Right Height", getHeightRight()); - // SmartDashboard.putNumber("[CLIMBER] Left Power", leftPower); - // SmartDashboard.putNumber("[CLIMBER] Right Power", rightPower); - // SmartDashboard.putNumber( - // "[CLIMBER] Left Motor Supply Current", MotorUtil.getCurrentUsage(leftMotor)); - // SmartDashboard.putNumber( - // "[CLIMBER] Right Motor Supply Current", MotorUtil.getCurrentUsage(rightMotor)); - // SmartDashboard.putNumber( - // "[CLIMBER] Left Motor Stator Current", - // MotorUtil.getStatorCurrentUsage(leftMotor)); - // SmartDashboard.putNumber( - // "[CLIMBER] Right Motor Stator Current", - // MotorUtil.getStatorCurrentUsage(rightMotor)); + ShuffleboardUtil.putNumber("[CLIMBER] Left Rotations", leftMotor.getSensorPosition()); + ShuffleboardUtil.putNumber("[CLIMBER] Right Rotations", rightMotor.getSensorPosition()); + ShuffleboardUtil.putNumber("[CLIMBER] Left Height", getHeightLeft()); + ShuffleboardUtil.putNumber("[CLIMBER] Right Height", getHeightRight()); + ShuffleboardUtil.putNumber("[CLIMBER] Left Power", leftPower); + ShuffleboardUtil.putNumber("[CLIMBER] Right Power", rightPower); + ShuffleboardUtil.putNumber( + "[CLIMBER] Left Motor Supply Current", MotorUtil.getCurrentUsage(leftMotor)); + ShuffleboardUtil.putNumber( + "[CLIMBER] Right Motor Supply Current", MotorUtil.getCurrentUsage(rightMotor)); + ShuffleboardUtil.putNumber( + "[CLIMBER] Left Motor Stator Current", MotorUtil.getStatorCurrentUsage(leftMotor)); + ShuffleboardUtil.putNumber( + "[CLIMBER] Right Motor Stator Current", + MotorUtil.getStatorCurrentUsage(rightMotor)); } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 7caa39d3f..0184876d4 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -4,6 +4,7 @@ import com.team766.ViSIONbase.GrayScaleCamera; import com.team766.framework.Mechanism; import com.team766.logging.LoggerExceptionUtils; +import com.team766.logging.ShuffleboardUtil; import com.team766.robot.reva.Robot; import com.team766.robot.reva.constants.VisionConstants; import edu.wpi.first.math.geometry.Transform3d; @@ -59,9 +60,9 @@ public void run() { Transform3d toUse = GrayScaleCamera.getBestTargetTransform3d(camera.getTrackedTargetWithID(tagId)); - // SmartDashboard.putNumber("x value SUIIII", toUse.getX()); - // SmartDashboard.putNumber("y value SUIIII", toUse.getY()); - } catch (Exception e) { + ShuffleboardUtil.putNumber("x value SUIIII", toUse.getX()); + ShuffleboardUtil.putNumber("y value SUIIII", toUse.getY()); + } catch (AprilTagGeneralCheckedException e) { return; } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java index 691a6dd49..be69d5412 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java @@ -10,6 +10,7 @@ import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.library.ValueProvider; +import com.team766.logging.ShuffleboardUtil; public class Intake extends Mechanism { @@ -98,10 +99,10 @@ public void nudgeDown() { } public void run() { - // SmartDashboard.putString("[INTAKE]", state.toString()); - // SmartDashboard.putNumber("[INTAKE POWER]", intakePower); - // SmartDashboard.putNumber("[INTAKE] Current", MotorUtil.getCurrentUsage(intakeMotor)); - // SmartDashboard.putNumber("Prox Sensor", sensor.getRange()); + ShuffleboardUtil.putString("[INTAKE]", state.toString()); + ShuffleboardUtil.putNumber("[INTAKE POWER]", intakePower); + ShuffleboardUtil.putNumber("[INTAKE] Current", MotorUtil.getCurrentUsage(intakeMotor)); + ShuffleboardUtil.putNumber("Prox Sensor", sensor.getRange()); } // feel free to refactor these two functions later - I didn't want to mess up existing code diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java index 953bb5b16..796201900 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java @@ -9,6 +9,7 @@ import com.team766.hal.MotorController.ControlMode; import com.team766.hal.RobotProvider; import com.team766.library.RateLimiter; +import com.team766.logging.ShuffleboardUtil; public class Shooter extends Mechanism { public static final double DEFAULT_SPEED = @@ -88,14 +89,14 @@ public void nudgeDown() { public void run() { if (speedUpdated || rateLimiter.next()) { - // SmartDashboard.putNumber("[SHOOTER TARGET SPEED]", shouldRun ? targetSpeed : 0.0); - // SmartDashboard.putNumber("[SHOOTER TOP MOTOR SPEED]", getShooterSpeedTop()); - // SmartDashboard.putNumber("[SHOOTER BOTTOM MOTOR SPEED]", getShooterSpeedBottom()); - // SmartDashboard.putNumber( - // "[SHOOTER] Top Motor Current", MotorUtil.getCurrentUsage(shooterMotorTop)); - // SmartDashboard.putNumber( - // "[SHOOTER] Bottom Motor Current", - // MotorUtil.getCurrentUsage(shooterMotorBottom)); + ShuffleboardUtil.putNumber("[SHOOTER TARGET SPEED]", shouldRun ? targetSpeed : 0.0); + ShuffleboardUtil.putNumber("[SHOOTER TOP MOTOR SPEED]", getShooterSpeedTop()); + ShuffleboardUtil.putNumber("[SHOOTER BOTTOM MOTOR SPEED]", getShooterSpeedBottom()); + ShuffleboardUtil.putNumber( + "[SHOOTER] Top Motor Current", MotorUtil.getCurrentUsage(shooterMotorTop)); + ShuffleboardUtil.putNumber( + "[SHOOTER] Bottom Motor Current", + MotorUtil.getCurrentUsage(shooterMotorBottom)); } // SmartDashboard.putBoolean("Shooter At Speed", isCloseToExpectedSpeed()); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 536781ffa..f976c2d7c 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -13,6 +13,7 @@ import com.team766.hal.RobotProvider; import com.team766.hal.wpilib.REVThroughBoreDutyCycleEncoder; import com.team766.library.ValueProvider; +import com.team766.logging.ShuffleboardUtil; public class Shoulder extends Mechanism { public enum ShoulderPosition { @@ -132,7 +133,7 @@ public void rotate(double angle) { com.team766.math.Math.clamp( angle, ShoulderPosition.BOTTOM.getAngle(), ShoulderPosition.TOP.getAngle()); targetRotations = degreesToRotations(targetAngle); - // SmartDashboard.putNumber("[SHOULDER Target Angle]", targetAngle); + ShuffleboardUtil.putNumber("[SHOULDER Target Angle]", targetAngle); // actual rotation will happen in run() } @@ -153,30 +154,28 @@ public void run() { leftMotor.setSensorPosition(convertedPos); encoderInitializationCount++; } - // SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); - // SmartDashboard.putNumber("[SHOULDER] Target Angle", targetAngle); - // SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations()); - // SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations); - // SmartDashboard.putNumber("[SHOULDER] Encoder Frequency", absoluteEncoder.getFrequency()); - // SmartDashboard.putNumber( - // "[SHOULDER] Absolute Encoder Position", getAbsoluteEncoderPosition()); - // SmartDashboard.putNumber( - // "[SHOULDER] Left Motor Supply Current", MotorUtil.getCurrentUsage(leftMotor)); - // SmartDashboard.putNumber( - // "[SHOULDER] Right Motor Supply Current", MotorUtil.getCurrentUsage(rightMotor)); - // SmartDashboard.putNumber( - // "[SHOULDER] Left Motor Stator Current", - // MotorUtil.getStatorCurrentUsage(leftMotor)); - // SmartDashboard.putNumber( - // "[SHOULDER] Right Motor Stator Current", - // MotorUtil.getStatorCurrentUsage(rightMotor)); - // SmartDashboard.putBoolean("Shoulder at angle", isFinished()); + ShuffleboardUtil.putNumber("[SHOULDER] Angle", getAngle()); + ShuffleboardUtil.putNumber("[SHOULDER] Target Angle", targetAngle); + ShuffleboardUtil.putNumber("[SHOULDER] Rotations", getRotations()); + ShuffleboardUtil.putNumber("[SHOULDER] Target Rotations", targetRotations); + ShuffleboardUtil.putNumber("[SHOULDER] Encoder Frequency", absoluteEncoder.getFrequency()); + ShuffleboardUtil.putNumber( + "[SHOULDER] Absolute Encoder Position", getAbsoluteEncoderPosition()); + ShuffleboardUtil.putNumber( + "[SHOULDER] Left Motor Supply Current", MotorUtil.getCurrentUsage(leftMotor)); + ShuffleboardUtil.putNumber( + "[SHOULDER] Right Motor Supply Current", MotorUtil.getCurrentUsage(rightMotor)); + ShuffleboardUtil.putNumber( + "[SHOULDER] Left Motor Stator Current", MotorUtil.getStatorCurrentUsage(leftMotor)); + ShuffleboardUtil.putNumber( + "[SHOULDER] Right Motor Stator Current", + MotorUtil.getStatorCurrentUsage(rightMotor)); TalonFX leftTalon = (TalonFX) leftMotor; - // SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get()); + ShuffleboardUtil.putNumber("[SHOULDER] ffGain", ffGain.get()); double ff = ffGain.valueOr(0.0) * Math.cos(Math.toRadians(getAngle())); - // SmartDashboard.putNumber("[SHOULDER] FF", ff); - // SmartDashboard.putNumber("[SHOULDER VELOCITY]", Math.abs(leftMotor.getSensorVelocity())); + ShuffleboardUtil.putNumber("[SHOULDER] FF", ff); + ShuffleboardUtil.putNumber("[SHOULDER VELOCITY]", Math.abs(leftMotor.getSensorVelocity())); PositionDutyCycle positionRequest = new PositionDutyCycle(targetRotations); positionRequest.FeedForward = ff; leftTalon.setControl(positionRequest); From 02383dbbab883abb984d888c5fc585d8fcbe6120 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Fri, 29 Mar 2024 22:47:57 -0700 Subject: [PATCH 2/8] fix compilation errors --- .../com/team766/logging/ShuffleboardUtil.java | 27 ++++++++++--------- .../robot/common/mechanisms/SwerveModule.java | 2 +- .../java/com/team766/robot/gatorade/OI.java | 2 +- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/java/com/team766/logging/ShuffleboardUtil.java b/src/main/java/com/team766/logging/ShuffleboardUtil.java index bf43bdcea..f49443eb4 100644 --- a/src/main/java/com/team766/logging/ShuffleboardUtil.java +++ b/src/main/java/com/team766/logging/ShuffleboardUtil.java @@ -1,29 +1,30 @@ package com.team766.logging; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.MatchType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public final class ShuffleboardUtil { - private static boolean ENABLE_LOGGING_IF_NOT_COMP = true; - private static MatchType matchType = null; + 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; - } + // if (ENABLE_LOGGING_IF_NOT_COMP) { + // return false; + // } - synchronized (matchType) { - if (matchType == MatchType.None) { - matchType = DriverStation.getMatchType(); - } - } + // TODO: test this more carefully when we have time. + // synchronized (matchType) { + // if (matchType == MatchType.None) { + // matchType = DriverStation.getMatchType(); + // } + // } - return matchType != MatchType.None; + // return matchType != MatchType.None; } public static void putNumber(String key, double value) { diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java index b41476589..b02ae2476 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java +++ b/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.ShuffleboardUtil.ShuffleboardUtil; import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; /** diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index f15b08d60..15f9538be 100644 --- a/src/main/java/com/team766/robot/gatorade/OI.java +++ b/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.ShuffleboardUtil.ShuffleboardUtil; /** * This class is the glue that binds the controls on the physical operator From 6621a16d33f1514a8dce151dc1a7a713ec847cdb Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Sun, 31 Mar 2024 16:30:23 -0700 Subject: [PATCH 3/8] fix merge issue --- .../team766/robot/reva/mechanisms/ForwardApriltagCamera.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 0184876d4..622310924 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -39,7 +39,6 @@ public GrayScaleCamera getCamera() { } public void run() { - try { if (tagId == -1) { Optional alliance = DriverStation.getAlliance(); @@ -62,7 +61,7 @@ public void run() { ShuffleboardUtil.putNumber("x value SUIIII", toUse.getX()); ShuffleboardUtil.putNumber("y value SUIIII", toUse.getY()); - } catch (AprilTagGeneralCheckedException e) { + } catch (Exception e) { return; } } From 69e300c283143f31f228e6f4b47503d8a3453022 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Sun, 31 Mar 2024 16:40:28 -0700 Subject: [PATCH 4/8] fixing merge issue --- .../mechanisms/ForwardApriltagCamera.java | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 622310924..89fc356ff 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -4,9 +4,7 @@ import com.team766.ViSIONbase.GrayScaleCamera; import com.team766.framework.Mechanism; import com.team766.logging.LoggerExceptionUtils; -import com.team766.logging.ShuffleboardUtil; import com.team766.robot.reva.Robot; -import com.team766.robot.reva.constants.VisionConstants; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -39,15 +37,31 @@ public GrayScaleCamera getCamera() { } public void run() { + if (tagId == -1) { + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isPresent()) { + if (alliance.get().equals(Alliance.Blue)) { + tagId = 7; + } else { + tagId = 4; + } + Robot.lights.signalCameraConnected(); + } else { + LoggerExceptionUtils.logException( + new AprilTagGeneralCheckedException("Couldn't find alliance correctly")); + } + } + try { if (tagId == -1) { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { if (alliance.get().equals(Alliance.Blue)) { - tagId = VisionConstants.MAIN_BLUE_SPEAKER_TAG; + tagId = 7; } else { - tagId = VisionConstants.MAIN_RED_SPEAKER_TAG; + tagId = 4; } Robot.lights.signalCameraConnected(); } else { @@ -59,8 +73,8 @@ public void run() { Transform3d toUse = GrayScaleCamera.getBestTargetTransform3d(camera.getTrackedTargetWithID(tagId)); - ShuffleboardUtil.putNumber("x value SUIIII", toUse.getX()); - ShuffleboardUtil.putNumber("y value SUIIII", toUse.getY()); + // SmartDashboard.putNumber("x value SUIIII", toUse.getX()); + // SmartDashboard.putNumber("y value SUIIII", toUse.getY()); } catch (Exception e) { return; } From 99f70282708ceade6b77784f5c7ae840b70f217a Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Sun, 31 Mar 2024 16:40:35 -0700 Subject: [PATCH 5/8] fixing merge issue --- .../team766/robot/reva/mechanisms/ForwardApriltagCamera.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 89fc356ff..e0a4a7544 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -4,6 +4,7 @@ import com.team766.ViSIONbase.GrayScaleCamera; import com.team766.framework.Mechanism; import com.team766.logging.LoggerExceptionUtils; +import com.team766.logging.ShuffleboardUtil; import com.team766.robot.reva.Robot; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; @@ -73,8 +74,8 @@ public void run() { Transform3d toUse = GrayScaleCamera.getBestTargetTransform3d(camera.getTrackedTargetWithID(tagId)); - // SmartDashboard.putNumber("x value SUIIII", toUse.getX()); - // SmartDashboard.putNumber("y value SUIIII", toUse.getY()); + ShuffleboardUtil.putNumber("x value SUIIII", toUse.getX()); + ShuffleboardUtil.putNumber("y value SUIIII", toUse.getY()); } catch (Exception e) { return; } From a0028fd6d329c357d06ab360913c1345fd14e665 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Thu, 4 Apr 2024 00:04:53 -0700 Subject: [PATCH 6/8] Merge remote-tracking branch 'origin/main' into shuffleboard-util From f0c2415a1bdb9ffa2bc126ccf55f7e82caf11fa0 Mon Sep 17 00:00:00 2001 From: Lydia Honerkamp <90945906+1yd1a@users.noreply.github.com> Date: Thu, 4 Apr 2024 22:37:50 -0700 Subject: [PATCH 7/8] enable logging to data log (#118) --- src/main/java/com/team766/hal/wpilib/RobotMain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index ad1486dd9..ce3e831b2 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -102,7 +102,7 @@ public void robotInit() { if (isReal()) { // enable dual-logging - com.team766.logging.Logger.enableLoggingToDataLog(false); + com.team766.logging.Logger.enableLoggingToDataLog(true); // set up AdvantageKit logging DataLogManager.log("Initializing logging."); From 572055da493f791345eb44eb71d5b25d5d28393d Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Thu, 4 Apr 2024 22:57:04 -0700 Subject: [PATCH 8/8] fix merge issue --- .../mechanisms/ForwardApriltagCamera.java | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index e0a4a7544..622310924 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -6,6 +6,7 @@ import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.ShuffleboardUtil; import com.team766.robot.reva.Robot; +import com.team766.robot.reva.constants.VisionConstants; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -38,31 +39,15 @@ public GrayScaleCamera getCamera() { } public void run() { - if (tagId == -1) { - Optional alliance = DriverStation.getAlliance(); - - if (alliance.isPresent()) { - if (alliance.get().equals(Alliance.Blue)) { - tagId = 7; - } else { - tagId = 4; - } - Robot.lights.signalCameraConnected(); - } else { - LoggerExceptionUtils.logException( - new AprilTagGeneralCheckedException("Couldn't find alliance correctly")); - } - } - try { if (tagId == -1) { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { if (alliance.get().equals(Alliance.Blue)) { - tagId = 7; + tagId = VisionConstants.MAIN_BLUE_SPEAKER_TAG; } else { - tagId = 4; + tagId = VisionConstants.MAIN_RED_SPEAKER_TAG; } Robot.lights.signalCameraConnected(); } else {