Merge branch 'post-wne' into auto-adjustments

Author: bobopop787

src/main/java/frc/robot/RobotContainer.java

@@ -106,7 +106,7 @@ private void configureBindings() {
 
         swerve.setToAimSuppliers(
             driverController.leftTrigger(), // aim reef
-            driverController.b(), // aim processor
+            () -> false, // aim processor, disabled
             driverController.a() // aim station
         );
         swerve.setReefChooserSuppliers(
@@ -128,7 +128,7 @@ private void configureBindings() {
         driverController.b().onTrue(swerve.runUpdateControlConstants().andThen(elevator.runUpdateControlConstants()));
         driverController.povLeft().onTrue(swerve.runTogglePresetPosition(PresetPositionType.LEFTREEF));
         driverController.povRight().onTrue(swerve.runTogglePresetPosition(PresetPositionType.RIGHTREEF));
-        driverController.povUp().onTrue(swerve.runTogglePresetPosition(PresetPositionType.PROCESSOR));
+        // driverController.povUp().onTrue(swerve.runTogglePresetPosition(PresetPositionType.PROCESSOR));
         driverController.leftBumper().onTrue(swerve.runSetPresetXEnabled(true));
         driverController.leftBumper().onFalse(swerve.runSetPresetXEnabled(false));
 

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -706,13 +706,16 @@ public void periodic() {
                 getPose().getY() - fieldZone.rightReefPose.getY(),
                 getPose().getRotation().minus(fieldZone.rightReefPose.getRotation())
             );
+            leftErrorPose.rotateAround(Translation2d.kZero, fieldZone.leftReefPose.getRotation().unaryMinus());
+            rightErrorPose.rotateAround(Translation2d.kZero, fieldZone.rightReefPose.getRotation().unaryMinus());
             if(desiredPresetPosition.equals(PresetPositionType.LEFTREEF) || desiredPresetPosition.equals(PresetPositionType.RIGHTREEF)) {
                 Pose2d desiredPose = desiredPresetPosition.equals(PresetPositionType.LEFTREEF) ? fieldZone.leftReefPose : fieldZone.rightReefPose;
                 Pose2d errorPose = new Pose2d(
                     getPose().getX() - desiredPose.getX(),
                     getPose().getY() - desiredPose.getY(),
                     getPose().getRotation().minus(desiredPose.getRotation())
                 );
+                errorPose.rotateAround(Translation2d.kZero, desiredPose.getRotation());
                 isAligned =
                     Math.abs(errorPose.getX()) < SwerveConstants.kReefAlignXTolerance &&
                     Math.abs(errorPose.getY()) < SwerveConstants.kReefAlignYTolerance &&
@@ -774,7 +777,7 @@ public void periodic() {
                 };
                 presetY = Constants.isRed() ? FieldConstants.fieldHeight - Units.inchesToMeters(16) : Units.inchesToMeters(16);
             case CORALSTATION:
-                // TODO add
+                // not implementing for competition
             default:
                 break;
         }