diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 98c2f5c9..0aa2fbd9 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -185,12 +185,12 @@ public SwerveDrive( setMaximumSpeed(maxSpeedMPS); // Initialize Telemetry - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { SmartDashboard.putData("Field", field); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; @@ -485,11 +485,11 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent } // Display commanded speed for testing - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO) { SmartDashboard.putString("RobotVelocity", velocity.toString()); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; @@ -660,7 +660,7 @@ public void resetOdometry(Pose2d pose) */ public void postTrajectory(Trajectory trajectory) { - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { field.getObject("Trajectory").setTrajectory(trajectory); } @@ -871,7 +871,7 @@ public void lockPose() { SwerveModuleState desiredState = new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = desiredState.angle.getDegrees(); @@ -932,7 +932,7 @@ public void updateOdometry() swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); // Update angle accumulator if the robot is simulated - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); if (SwerveDriveTelemetry.isSimulation) @@ -951,7 +951,7 @@ public void updateOdometry() SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees(); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); } @@ -967,7 +967,7 @@ public void updateOdometry() SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees()); SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees()); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees(); SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond; @@ -983,7 +983,7 @@ public void updateOdometry() moduleSynchronizationCounter = 0; } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.updateData(); } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 61bb5914..c7dd56c1 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -366,7 +366,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, simModule.updateStateAndPosition(desiredState); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees(); SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity; diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 8a300d96..26094d3d 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -125,6 +125,14 @@ public enum TelemetryVerbosity * Low telemetry data, only post the robot position on the field. */ LOW, + /** + * Medium telemetry data, swerve directory + */ + INFO, + /** + * Info level + field info + */ + POSE, /** * Full swerve drive data is sent back in both human and machine readable forms. */