From d6824df962490b61128ab4f45dad630480b9e54b Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Wed, 25 Mar 2026 22:02:50 -0400 Subject: [PATCH] Chassis Align improvements --- lib/RoboLib | 2 +- .../frc/robot/commands/AutoShootCommandChassisTurret.java | 5 +++-- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 4 +--- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/lib/RoboLib b/lib/RoboLib index aad75b9..cd9dee7 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit aad75b9b559825aaf60ad5bb550bf8263d3d203d +Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12 diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 3b4d2d7..38270a7 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -83,8 +83,9 @@ public class AutoShootCommandChassisTurret extends Command{ List tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList(); Pose3d closest = botPose.nearest(tagPoses); Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest); - Transform3d delta = botPose.minus(closest); - driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? -chassisRotateSpeed : chassisRotateSpeed); // May need to flip this + Transform3d delta = closest.minus(botPose); + Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); + driveSubsystem.rotateChassis(delta.getRotation().toRotation2d().getDegrees() > 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this } else // We have an april tag, center it to the camera frame { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d16224d..485a5f0 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -135,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - // driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999)); this.setDefaultCommand(Commands.run(new Runnable() { @@ -146,8 +146,6 @@ public class DriveSubsystem extends TestableSubsystem { -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())), -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4)))); - // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0)); - Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates()); Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates()); Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose3d());