Log odometry in parent class

lasarobotics · Dec 30, 2024 · 8832d4d · 8832d4d
1 parent dc4c820
commit 8832d4d
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 7 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/swerve/SwerveModule.java b/src/main/java/org/lasarobotics/drive/swerve/SwerveModule.java
@@ -13,6 +13,7 @@
 
 import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
 import org.lasarobotics.drive.TractionControlController;
+import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -73,8 +74,11 @@ public Rotation2d getLockPosition() {
     }
   }
 
+  private static final String ODOMETER_LOG_ENTRY = "/Odometer";
+
   private double m_runningOdometer;
   private String m_odometerOutputPath;
+  private String m_driveMotorName;
 
   private Location m_location;
   private GearRatio m_gearRatio;
@@ -98,6 +102,7 @@ public SwerveModule(Location location,
     this.m_driveWheel = driveWheel;
     this.m_zeroOffset = new Rotation2d(zeroOffset);
     this.m_runningOdometer = 0.0;
+    this.m_driveMotorName = driveMotorName;
     this.m_autoLock = true;
     COSINE_CORRECTION = RobotBase.isReal() ? 1 : 0;
 
@@ -263,6 +268,15 @@ protected SwerveModuleState getDesiredState(SwerveModuleState requestedState, Ro
     return desiredState;
   }
 
+  /**
+   * Log outputs
+   * <p>
+   * Call this in child class
+   */
+  protected void logOutputs() {
+    Logger.recordOutput(m_driveMotorName + ODOMETER_LOG_ENTRY, m_runningOdometer);
+  }
+
   /**
    * Get drive wheel installed on module
    * @return Drive wheel

diff --git a/src/main/java/org/lasarobotics/drive/swerve/parent/CTRESwerveModule.java b/src/main/java/org/lasarobotics/drive/swerve/parent/CTRESwerveModule.java
@@ -76,7 +76,6 @@ public Hardware(TalonFX driveMotor, TalonFX rotateMotor, CANcoder canCoder) {
   private final Current ROTATE_MOTOR_CURRENT_LIMIT = Units.Amps.of(20.0);
 
   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 String MAX_LINEAR_VELOCITY_LOG_ENTRY = "/MaxLinearVelocity";
   private static final double MAX_AUTO_LOCK_TIME = 10.0;
@@ -104,7 +103,6 @@ public Hardware(TalonFX driveMotor, TalonFX rotateMotor, CANcoder canCoder) {
   private SwerveModule.GearRatio m_gearRatio;
   private double m_driveConversionFactor;
   private double m_autoLockTime;
-  private double m_runningOdometer;
   private final boolean m_isPhoenixPro;
 
   private Instant m_autoLockTimer;
@@ -172,7 +170,6 @@ public CTRESwerveModule(Hardware swerveHardware,
     this.m_autoLockTime = MathUtil.clamp(autoLockTime.in(Units.Milliseconds), 0.0, MAX_AUTO_LOCK_TIME * 1000);
     this.m_previousRotatePosition = m_zeroOffset.plus(m_location.getLockPosition());
     this.m_autoLockTimer = Instant.now();
-    this.m_runningOdometer = 0.0;
     this.m_rotatePositionSetter = new PositionVoltage(Units.Radians.zero());
     this.m_driveVelocitySetter = new VelocityVoltage(Units.RotationsPerSecond.zero());
 
@@ -328,8 +325,8 @@ private AngularVelocity linearToAngularVelocity(LinearVelocity velocity) {
    * Call this method periodically
    */
   private void periodic() {
+    super.logOutputs();
     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().selectedSensorPosition.in(Units.Radians))));
   }
 
@@ -498,7 +495,7 @@ public void set(SwerveModuleState state) {
     m_previousRotatePosition = m_desiredState.angle;
 
     // Increment odometer
-    m_runningOdometer += Math.abs(m_desiredState.speedMetersPerSecond) * GlobalConstants.ROBOT_LOOP_HZ.asPeriod().in(Units.Seconds);
+    super.incrementOdometer(Math.abs(m_desiredState.speedMetersPerSecond) * GlobalConstants.ROBOT_LOOP_HZ.asPeriod().in(Units.Seconds));
   }
 
   @Override

diff --git a/src/main/java/org/lasarobotics/drive/swerve/parent/REVSwerveModule.java b/src/main/java/org/lasarobotics/drive/swerve/parent/REVSwerveModule.java
@@ -73,7 +73,6 @@ public Hardware(Spark driveMotor, Spark rotateMotor) {
   private final Current ROTATE_MOTOR_CURRENT_LIMIT = Units.Amps.of(20.0);
 
   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 String MAX_LINEAR_VELOCITY_LOG_ENTRY = "/MaxLinearVelocity";
   private static final double MAX_AUTO_LOCK_TIME = 10.0;
@@ -280,8 +279,8 @@ void updateSimPosition() {
    * Call this method periodically
    */
   private void periodic() {
+    super.logOutputs();
     Logger.recordOutput(m_driveMotor.getID().name + IS_SLIPPING_LOG_ENTRY, isSlipping());
-    Logger.recordOutput(m_driveMotor.getID().name + ODOMETER_LOG_ENTRY, getRunningOdometer());
     Logger.recordOutput(m_rotateMotor.getID().name + ROTATE_ERROR_LOG_ENTRY, m_desiredState.angle.minus(Rotation2d.fromRadians(m_rotateMotor.getInputs().absoluteEncoderPosition)));
   }