From 796a4c38f98c56b6dd834b17b994c64bceb9521e Mon Sep 17 00:00:00 2001
From: penguin212 <dhuber06@gmail.com>
Date: Mon, 1 Apr 2024 17:43:21 -0700
Subject: [PATCH] finished commands

---
 src/main/java/frc/robot/Constants.java      |  2 +-
 src/main/java/frc/robot/RobotContainer.java | 69 +++++++++++++--------
 2 files changed, 44 insertions(+), 27 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 0ce0c6a7..953b0b75 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -133,7 +133,7 @@ public static class ShooterConstants {
         public static final double FLYWHEEL_SHUTTLE_SPEED = 0.3;
 
         //center of red speaker: (652.73 218.42)
-        public static final double RED_X = Units.inchesToMeters(652.73 + 9.05);
+        public static final double RED_X = Units.inchesToMeters(652.73 - 9.05);
         public static final double RED_Y = Units.inchesToMeters(218.42);
 
         //center of blue speaker: (-1.50 218.42)
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5c21f3a7..fc4f7f39 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
@@ -36,6 +37,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
+import frc.robot.Constants.SwerveConstants;
 import frc.robot.commands.auton.AutonBuilder;
 import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.climb.ClimbRaiseCommand;
@@ -257,6 +259,46 @@ private void configureBindings() {
             swerveSubsystem.resetDriverHeading();
         }));
         
+        /* SWERVE BINDINGS */
+
+        /* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
+        // drive
+
+        /* Amp Align -- Pressing and holding the button will cause the robot to automatically path find to the amp.
+         * Releasing the button will stop the robot (and the path finding). */
+        driveController.getAmpAlign().onTrue(new InstantCommand(
+            () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
+            ).andThen(new ParallelRaceGroup(
+                AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
+                new ConditionalWaitCommand(
+                    () -> !driveController.getAmpAlign().getAsBoolean()))
+                    ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
+        );
+
+        /* Note align -- deprecated, new version in the works*/
+        driveController.getNoteAlign().onTrue(
+            new NoteAlignCommand(swerveSubsystem, noteDetector, driveController)
+                .unless(() -> noteDetector.getNote().isEmpty())
+        );
+    
+        /* Swerve Stop -- Pressing the button completely stops the robot's motion. */
+        driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));
+
+        driveController.getShooterAimButton().onTrue(Commands.runOnce(
+            () -> {
+                swerveSubsystem.setTargetPoint(fmsSubsystem.isRedAlliance() ? SwerveConstants.RED_SPEAKER_POS : SwerveConstants.BLUE_SPEAKER_POS);
+                swerveSubsystem.setAim(true);
+            }
+        ));
+
+        driveController.getShooterAimButton().onFalse(Commands.runOnce(
+            () -> {
+                swerveSubsystem.setAim(false);
+            }
+        ));
+
+        
+
         /* SHOOTER PIVOT TEST */
 
         // rightBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem,
@@ -543,32 +585,7 @@ private void configureBindings() {
             () -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0)))
         );
 
-        /* SWERVE BINDINGS */
-
-        /* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
-        // drive
-
-        /* Amp Align -- Pressing and holding the button will cause the robot to automatically path find to the amp.
-         * Releasing the button will stop the robot (and the path finding). */
-        driveController.getAmpAlign().onTrue(new InstantCommand(
-            () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
-            ).andThen(new ParallelRaceGroup(
-                AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
-                new ConditionalWaitCommand(
-                    () -> !driveController.getAmpAlign().getAsBoolean()))
-                    ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
-        );
-
-        /* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
-        driveController.getNoteAlign().onTrue(
-            new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem,
-                                   swerveSubsystem, noteDetector,
-                                   driveController, lightBarSubsystem)                  
-            .onlyWhile(driveController.getNoteAlign())
-        );
-    
-        /* Swerve Stop -- Pressing the button completely stops the robot's motion. */
-        driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem));
+        
 
         
     }