final controlls except auto align and lights but thats tommrows problem

Co-authored-by: Raymond Dokholyan <urayed@users.noreply.github.com>
FRC2539 · Feb 22, 2025 · cfe0464 · cfe0464
1 parent 36f6164
commit cfe0464
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 19 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -359,6 +359,7 @@ private void configureBindings() {
         operatorController.getLeftBumper().onTrue(stateManager.setLeftCoralMode());
         operatorController.getRightBumper().onTrue(stateManager.setRightCoralMode());
         operatorController.getRightTrigger().onTrue(stateManager.setAlgaeMode());
+
         leftDriveController.getRightTopRight().onTrue(stateManager.setArmWristMode());
         operatorController
                 .getLeftJoystick()
@@ -368,7 +369,7 @@ private void configureBindings() {
                                         LightsSubsystem.LEDSegment.MainStrip.setRainbowAnimation(
                                                 1)))); // L3 Rainbow
         operatorController
-                .getLeftTrigger()
+                .getRightJoystick()
                 .whileTrue(
                         Commands.runOnce(
                                 (() ->
@@ -389,6 +390,8 @@ private void configureBindings() {
         CORAL.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2));
         CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1));
 
+        operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Climb));
+
         bindPlaceSeq(operatorController.getY(), Position.L4Prep, Position.L4, 0.1);
 
         bindPlaceSeq(operatorController.getX(), Position.L3Prep, Position.L3, 0.1);
@@ -441,9 +444,12 @@ private void configureBindings() {
         //         .whileTrue(intakeSubsystem.openAndRun().alongWith(alignToPiece()));
         // rightDriveController.getRightThumb().whileTrue(intakeSubsystem.openAndEject());
 
-        CORAL.and(rightDriveController.getBottomThumb())
+        CORAL.and(rightDriveController.getLeftThumb())
                 .whileTrue(gripperSubsystem.intakeSpinCoral());
-        CORAL.and(DRIVER_TRIGGER.and(normalRelease)).whileTrue(gripperSubsystem.ejectSpinCoral());
+        CORAL.and(rightDriveController.getRightThumb())
+                .whileTrue(
+                        gripperSubsystem
+                                .ejectSpinCoral()); // and(normalRelease)).whileTrue(gripperSubsystem.ejectSpinCoral());
 
         ALGAE.and(rightDriveController.getBottomThumb())
                 .whileTrue(gripperSubsystem.intakeSpinAlgae());
@@ -522,21 +528,19 @@ private void configureBindings() {
     private void bindPlaceSeq(Trigger button, Position prep, Position end, double timeout) {
         (button)
                 .onTrue(
-                        (stateManager
-                                        .moveToPosition(prep)
-                                        .until(AUTO_DRIVER_TRIGGER)
-                                        .andThen(
-                                                stateManager
-                                                        .moveToPosition(end)
-                                                        .alongWith(
-                                                                Commands.waitSeconds(timeout)
-                                                                        .andThen(
-                                                                                gripperSubsystem
-                                                                                        .ejectSpinCoral()))
-                                                        .until(AUTO_DRIVER_TRIGGER.negate())))
-                                .repeatedly()
-                                .beforeStarting(() -> normalRelease.setPressed(false))
-                                .finallyDo(() -> normalRelease.setPressed(true)));
+                        stateManager
+                                .moveToPosition(prep)
+                                .until(AUTO_DRIVER_TRIGGER)
+                                .andThen(stateManager.moveToPosition(end)));
+        //                         .alongWith(
+        //                                 Commands.waitSeconds(timeout)
+        //                                         .andThen(
+        //                                                 gripperSubsystem
+        //                                                         .ejectSpinCoral()))
+        //                         .until(AUTO_DRIVER_TRIGGER.negate())))
+        // .repeatedly()
+        // .beforeStarting(() -> normalRelease.setPressed(false))
+        // .finallyDo(() -> normalRelease.setPressed(true)));
     }
 
     private double deadband(double value, double deadband) {

diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java
@@ -109,7 +109,7 @@ public static enum Position {
             NetAlgae(5, 1, 1, NetAlgaePrep),
             Source(1, -2, 1, None),
             AlgaeHome(1, 1, 1, None),
-            Climb(1, 1, 1, ChuteUpNull),
+            Climb(0, 0, 1.58, ChuteUpNull),
             Processor(1, 1, 1, ChuteUpNull),
             IcecreamCoral(1, 1, 1, ChuteUpNull),
             IcecreamAlgae(1, 1, 1, ChuteUpNull),