Raise arm to halfway prepare prepare setpoint with driver x (#158)

* Raise arm to halfway prepare prepare setpoint with driver x * Driver should toggle arm halfway up * Fix this --------- Co-authored-by: Cameron Earle <camearle20@me.com>
Mechanical-Advantage · Mar 9, 2024 · b85fb55 · b85fb55
1 parent 817763d
commit b85fb55
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 15 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -310,6 +310,7 @@ public RobotContainer() {
                   < Units.feetToMeters(25.0)
               && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED
               && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_CLIMB
+              && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB
               && superstructure.getCurrentGoal() != Superstructure.Goal.CLIMB
               && superstructure.getCurrentGoal() != Superstructure.Goal.TRAP
               && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB
@@ -609,20 +610,10 @@ private void configureButtonBindings() {
             () ->
                 superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_CLIMB
                     && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB)
-        .whileTrue(
+        .toggleOnTrue(
             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(), // No function yet
                 () -> 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/rollers/backpack/Backpack.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java
@@ -22,7 +22,7 @@ public class Backpack extends GenericRollerSystem<Backpack.Goal> {
   public enum Goal implements VoltageGoal {
     IDLING(() -> 0),
     AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage", 12.0)),
-    TRAP_SCORING(new LoggedTunableNumber("Backpack/TrapScoringVoltage", 2.0)),
+    TRAP_SCORING(new LoggedTunableNumber("Backpack/TrapScoringVoltage", 8.0)),
     EJECTING(new LoggedTunableNumber("Backpack/EjectingVoltage", -12.0));
 
     private final DoubleSupplier voltageSupplier;

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
@@ -32,6 +32,7 @@ public enum Goal {
     SUBWOOFER,
     PODIUM,
     RESET_CLIMB,
+    PREPARE_PREPARE_TRAP_CLIMB,
     PREPARE_CLIMB,
     CANCEL_PREPARE_CLIMB,
     CLIMB,
@@ -65,6 +66,7 @@ public void periodic() {
 
     // Retract climber
     if (!climber.retracted()
+        && desiredGoal != Goal.PREPARE_PREPARE_TRAP_CLIMB
         && desiredGoal != Goal.PREPARE_CLIMB
         && desiredGoal != Goal.CLIMB
         && desiredGoal != Goal.TRAP
@@ -135,6 +137,11 @@ public void periodic() {
         }
         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
@@ -82,6 +82,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)),
     RESET_CLIMB(new LoggedTunableNumber("Arm/ResetClimbDegrees", 30.0)),