From e16a4e018f66d96b6f86b3cfac6e13a53d2d03f0 Mon Sep 17 00:00:00 2001 From: Doug Wegscheid Date: Tue, 4 Feb 2025 12:03:42 -0500 Subject: [PATCH] Add instrumentation. --- .../frc/robot/subsystems/DriveSubsystem.java | 12 ++++++---- .../frc/robot/subsystems/MAXSwerveModule.java | 24 +++++++++++++++---- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 3cb2254e..ab392673 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -24,22 +24,26 @@ public class DriveSubsystem extends SubsystemBase { private final MAXSwerveModule m_frontLeft = new MAXSwerveModule( DriveConstants.kFrontLeftDrivingCanId, DriveConstants.kFrontLeftTurningCanId, - DriveConstants.kFrontLeftChassisAngularOffset); + DriveConstants.kFrontLeftChassisAngularOffset, + "FL"); private final MAXSwerveModule m_frontRight = new MAXSwerveModule( DriveConstants.kFrontRightDrivingCanId, DriveConstants.kFrontRightTurningCanId, - DriveConstants.kFrontRightChassisAngularOffset); + DriveConstants.kFrontRightChassisAngularOffset, + "FR"); private final MAXSwerveModule m_rearLeft = new MAXSwerveModule( DriveConstants.kRearLeftDrivingCanId, DriveConstants.kRearLeftTurningCanId, - DriveConstants.kBackLeftChassisAngularOffset); + DriveConstants.kBackLeftChassisAngularOffset, + "RL"); private final MAXSwerveModule m_rearRight = new MAXSwerveModule( DriveConstants.kRearRightDrivingCanId, DriveConstants.kRearRightTurningCanId, - DriveConstants.kBackRightChassisAngularOffset); + DriveConstants.kBackRightChassisAngularOffset, + "RR"); // The gyro sensor private final ADIS16470_IMU m_gyro = new ADIS16470_IMU(); diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index b2df0242..4cf78a5a 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; @@ -25,6 +26,7 @@ public class MAXSwerveModule { private final RelativeEncoder m_drivingEncoder; private final AbsoluteEncoder m_turningEncoder; + private final RelativeEncoder m_turningRelativeEncoder; // just used for debugging private final SparkClosedLoopController m_drivingClosedLoopController; private final SparkClosedLoopController m_turningClosedLoopController; @@ -32,18 +34,21 @@ public class MAXSwerveModule { private double m_chassisAngularOffset = 0; private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); + private String m_name; + /** * Constructs a MAXSwerveModule and configures the driving and turning motor, * encoder, and PID controller. This configuration is specific to the REV * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore * Encoder. */ - public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { + public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset, String name) { m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless); m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); m_drivingEncoder = m_drivingSpark.getEncoder(); m_turningEncoder = m_turningSpark.getAbsoluteEncoder(); + m_turningRelativeEncoder = m_turningSpark.getEncoder(); m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController(); m_turningClosedLoopController = m_turningSpark.getClosedLoopController(); @@ -59,6 +64,8 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular m_chassisAngularOffset = chassisAngularOffset; m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition()); m_drivingEncoder.setPosition(0); + + m_name = name; } /** @@ -76,14 +83,18 @@ public SwerveModuleState getState() { /** * Returns the current position of the module. * - * @return The current position of the module. + * @return The current position of the module */ public SwerveModulePosition getPosition() { // Apply chassis angular offset to the encoder position to get the position // relative to the chassis. - return new SwerveModulePosition( - m_drivingEncoder.getPosition(), - new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); + double drivingPosition = m_drivingEncoder.getPosition(); + double turningPosition = m_turningEncoder.getPosition() - m_chassisAngularOffset; + SmartDashboard.putNumber("swerve/" + m_name + "/drive/position", drivingPosition); + SmartDashboard.putNumber("swerve/" + m_name + "/drive/actual", m_drivingEncoder.getVelocity()); + SmartDashboard.putNumber("swerve/" + m_name + "/turn/actual", turningPosition); + SmartDashboard.putNumber("swerve/" + m_name + "/turn/relativeactual", m_turningRelativeEncoder.getPosition()); + return new SwerveModulePosition(drivingPosition, new Rotation2d(turningPosition)); } /** @@ -104,6 +115,9 @@ public void setDesiredState(SwerveModuleState desiredState) { m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity); m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition); + SmartDashboard.putNumber("swerve/" + m_name + "/drive/set", correctedDesiredState.speedMetersPerSecond); + SmartDashboard.putNumber("swerve/" + m_name + "/turn/set", correctedDesiredState.angle.getDegrees()); + m_desiredState = desiredState; }