From 05572caa189c85175a92d132048a8da96355cc66 Mon Sep 17 00:00:00 2001 From: CrumblzTheEgg Date: Wed, 4 Jun 2025 07:22:46 -0500 Subject: [PATCH] feat: added logging in the drive subsystem for advantage scope --- .../frc/robot/subsystems/DriveSubsystem.java | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 7bafa77..530282a 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -27,7 +27,11 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.event.NetworkBooleanEvent; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -943,6 +947,17 @@ public void updateOdometry() { SmartDashboard.putNumber("Theta Rotation", getRobotPose().getRotation().getRadians()); } + StructArrayPublisher targetStatesPublisher = NetworkTableInstance.getDefault() +.getStructArrayTopic("TargetStates", SwerveModuleState.struct).publish(); + StructArrayPublisher statesPublisher = NetworkTableInstance.getDefault() +.getStructArrayTopic("States", SwerveModuleState.struct).publish(); + StructPublisher targetSpeedsPublisher = NetworkTableInstance.getDefault() +.getStructTopic("TargetSpeeds", ChassisSpeeds.struct).publish(); + StructPublisher speedsPublisher = NetworkTableInstance.getDefault() +.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + StructPublisher posePublisher = NetworkTableInstance.getDefault() + .getStructTopic("Pose",Pose2d.struct).publish(); + /** * Periodically updates the drive subsystem. *

@@ -959,6 +974,12 @@ public void periodic() { // Update module states using the target velocities. setModules(targetVelocities); + targetStatesPublisher.set(setpoint.moduleStates()); + statesPublisher.set(getSwerveModuleStates()); + targetSpeedsPublisher.set(ChassisSpeeds.fromFieldRelativeSpeeds(targetVelocities,getRobotPose().getRotation())); + speedsPublisher.set(DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(getSwerveModuleStates())); + posePublisher.set(getRobotPose()); + // Refresh dynamic pathfinding obstacles. /*pathfindingObstacles.clear(); pathfindingObstaclesSuppliers.forEach(supplier -> pathfindingObstacles.addAll(supplier.get()));