Raise arm to halfway prepare prepare setpoint with driver x

Mechanical-Advantage · Mar 7, 2024 · 79c8996 · 79c8996
1 parent f917e74
commit 79c8996
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 13 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -546,18 +546,8 @@ private void configureButtonBindings() {
                     && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB)
         .whileTrue(
             Commands.either(
-                ClimbingCommands.autoDrive(
-                    false,
-                    drive,
-                    () -> -driver.getLeftY(),
-                    () -> -driver.getLeftX(),
-                    autoDriveDisable),
-                ClimbingCommands.autoDrive(
-                    true,
-                    drive,
-                    () -> -driver.getLeftY(),
-                    () -> -driver.getLeftX(),
-                    autoDriveDisable),
+                superstructure.setGoalCommand(Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB),
+                Commands.none(),
                 () -> trapScoreMode));
 
     // ------------- Operator Controls -------------

diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
@@ -34,7 +34,7 @@ public class ClimbingCommands {
   private static final LoggedTunableNumber climbedXOffset =
       new LoggedTunableNumber("ClimbingCommands/ClimbedXOffset", 0.16);
   private static final LoggedTunableNumber chainToBack =
-      new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.65);
+      new LoggedTunableNumber("ClimbingCommands/ChainToBackOffset", 0.3);
   private static final LoggedTunableNumber chainToFront =
       new LoggedTunableNumber("ClimbingCommands/ChainToFrontOffset", 0.9);
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -31,6 +31,7 @@ public enum Goal {
     SUBWOOFER,
     PODIUM,
     RESET_CLIMB,
+    PREPARE_PREPARE_TRAP_CLIMB,
     PREPARE_CLIMB,
     CANCEL_PREPARE_CLIMB,
     CLIMB,
@@ -123,6 +124,11 @@ public void periodic() {
         climber.setGoal(Climber.Goal.IDLE);
         backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
       }
+      case PREPARE_PREPARE_TRAP_CLIMB -> {
+        arm.setGoal(Arm.Goal.PREPARE_PREPARE_TRAP_CLIMB);
+        climber.setGoal(Climber.Goal.EXTEND);
+        backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
+      }
       case PREPARE_CLIMB -> {
         arm.setGoal(Arm.Goal.PREPARE_CLIMB);
         climber.setGoal(Climber.Goal.EXTEND);

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
@@ -81,6 +81,7 @@ public enum Goal {
     AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)),
     SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)),
     PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 30.0)),
+    PREPARE_PREPARE_TRAP_CLIMB(new LoggedTunableNumber("Arm/PreparePrepareTrapClimbDegrees", 40.0)),
     PREPARE_CLIMB(new LoggedTunableNumber("Arm/PrepareClimbDegrees", 105.0)),
     CLIMB(new LoggedTunableNumber("Arm/ClimbDegrees", 90.0)),
     CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0));