[swerve] Refactor Telemetry (#73)

src/main/java/frc/robot/Robot.java

@@ -44,7 +44,7 @@ public class Robot extends TimedRobot {
           .withRotationalDeadband(Constants.DrivebaseMaxAngularRate * 0.1)
           .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
   private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
-  private final Telemetry logger = new Telemetry(Constants.DrivebaseMaxSpeed);
+  private final Telemetry logger = new Telemetry();
 
   private void configureBindings() {
     m_drivetrain.setDefaultCommand(

src/main/java/frc/robot/Telemetry.java

@@ -7,25 +7,20 @@
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StructArrayPublisher;
 
-@SuppressWarnings("PMD.UnusedPrivateField")
 public class Telemetry {
-  private final double MaxSpeed;
-
-  /**
-   * Construct a telemetry object, with the specified max speed of the robot
-   *
-   * @param maxSpeed Maximum speed in meters per second
-   */
-  public Telemetry(double maxSpeed) {
-    MaxSpeed = maxSpeed;
-  }
+  /** Construct a telemetry object. */
+  public Telemetry() {}
 
   /* What to publish over networktables for telemetry */
   private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
@@ -39,8 +34,12 @@ public Telemetry(double maxSpeed) {
   private final NetworkTable driveStats = inst.getTable("Drive");
   private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
   private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
+  private final DoublePublisher omega = driveStats.getDoubleTopic("Omega").publish();
   private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
   private final DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();
+  private final DoublePublisher rotation = driveStats.getDoubleTopic("Rotation").publish();
+  StructArrayPublisher<SwerveModuleState> states =
+      driveStats.getStructArrayTopic("SwerveModuleStates", SwerveModuleState.struct).publish();
 
   /* Keep a reference of the last pose to calculate the speeds */
   private Pose2d m_lastPose = new Pose2d();
@@ -57,14 +56,18 @@ public void telemeterize(SwerveDriveState state) {
     double currentTime = Utils.getCurrentTimeSeconds();
     double diffTime = currentTime - lastTime;
     lastTime = currentTime;
-    Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation();
+    Transform2d poseDiff = pose.minus(m_lastPose);
     m_lastPose = pose;
 
-    Translation2d velocities = distanceDiff.div(diffTime);
+    Translation2d velocities = poseDiff.getTranslation().div(diffTime);
+    Rotation2d rotational_velocity = poseDiff.getRotation().div(diffTime);
 
     speed.set(velocities.getNorm());
     velocityX.set(velocities.getX());
     velocityY.set(velocities.getY());
+    omega.set(rotational_velocity.getRadians());
     odomPeriod.set(state.OdometryPeriod);
+    states.set(state.ModuleStates);
+    rotation.set(state.Pose.getRotation().getRadians());
   }
 }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -13,11 +13,6 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.networktables.DoublePublisher;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
@@ -39,10 +34,6 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
   private final PIDController m_yController = new PIDController(10, 0, 0.5);
   private final PIDController m_rotationController = new PIDController(7, 0, 0.35);
   private final ChoreoControlFunction m_swerveController;
-  private final NetworkTable drive_entry = NetworkTableInstance.getDefault().getTable("Drive");
-  StructArrayPublisher<SwerveModuleState> m_moduleStatesPublisher =
-      drive_entry.getStructArrayTopic("SwerveModuleStates", SwerveModuleState.struct).publish();
-  DoublePublisher m_rotationPublisher = drive_entry.getDoubleTopic("Rotation").publish();
 
   /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
   private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
@@ -135,11 +126,15 @@ public void periodic() {
                 hasAppliedOperatorPerspective = true;
               });
     }
-
-    m_moduleStatesPublisher.set(m_moduleStates);
-    m_rotationPublisher.set(m_odometry.getEstimatedPosition().getRotation().getRadians());
   }
 
+  /**
+   * Follow the given trajectory.
+   *
+   * @param name The name of the trajectory.
+   * @param isRed The perspective of the trajectory.
+   * @return A Command to follow the given trajectory.
+   */
   public Command followTrajectory(String name, boolean isRed) {
     var traj = Choreo.getTrajectory(name);
     var initPose = traj.getInitialPose();