From 8832d4de4bf4530991b732ebfca56cf9f21afaf6 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Mon, 30 Dec 2024 12:39:32 -0500 Subject: [PATCH] Log odometry in parent class --- .../lasarobotics/drive/swerve/SwerveModule.java | 14 ++++++++++++++ .../drive/swerve/parent/CTRESwerveModule.java | 7 ++----- .../drive/swerve/parent/REVSwerveModule.java | 3 +-- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/lasarobotics/drive/swerve/SwerveModule.java b/src/main/java/org/lasarobotics/drive/swerve/SwerveModule.java index 1b15c2d..af0c0bd 100644 --- 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 + *

+ * 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 index 4d5c81d..4ecd4d3 100644 --- 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 index ab72ea4..fd7ef36 100644 --- 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))); }