Merge pull request #185 from FRC5010/dev

Add additional logging levels INFO and POSE
BroncBotz3481 · Mar 6, 2024 · ba5069c · ba5069c
2 parents 425b421 + e517e38
commit ba5069c
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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.
      */