Log MAXSwerveModule rotation error

lasarobotics · Nov 6, 2024 · 0cd6ecc · 0cd6ecc
1 parent c80d79c
commit 0cd6ecc
Showing 1 changed file with 2 additions and 0 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/swerve/MAXSwerveModule.java b/src/main/java/org/lasarobotics/drive/swerve/MAXSwerveModule.java
@@ -104,6 +104,7 @@ private GearRatio(double value) {
 
   private static final String IS_SLIPPING_LOG_ENTRY = "/IsSlipping";
   private static final String ODOMETER_LOG_ENTRY = "/Odometer";
+  private static final String ROTATE_ERROR_LOG_ENTRY = "/RotateError";
   private static final double DRIVETRAIN_EFFICIENCY = 0.90;
   private static final double MAX_AUTO_LOCK_TIME = 10.0;
   private final double DRIVE_TICKS_PER_METER;
@@ -413,6 +414,7 @@ private Measure<Velocity<Distance>> getParallelVelocity(SwerveModuleState state,
   private void periodic() {
     Logger.recordOutput(m_driveMotor.getID().name + IS_SLIPPING_LOG_ENTRY, isSlipping());
     Logger.recordOutput(m_driveMotor.getID().name + ODOMETER_LOG_ENTRY, m_runningOdometer);
+    Logger.recordOutput(m_rotateMotor.getID().name + ROTATE_ERROR_LOG_ENTRY, m_desiredState.angle.minus(Rotation2d.fromRadians(m_rotateMotor.getInputs().absoluteEncoderPosition)));
   }
 
  /**