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..f49443eb4 --- /dev/null +++ b/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); + } +} 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..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.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)); } } diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index 7e7124bc7..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.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 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..622310924 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; @@ -38,7 +39,6 @@ public GrayScaleCamera getCamera() { } public void run() { - try { if (tagId == -1) { Optional alliance = DriverStation.getAlliance(); @@ -59,8 +59,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; } 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);