Climb Commands

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

@@ -12,10 +12,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.commands.CommandConstants;
-import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
-import frc.trigon.robot.commands.commandfactories.CoralEjectionCommands;
-import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
-import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.commands.commandfactories.*;
 import frc.trigon.robot.constants.CameraConstants;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
@@ -109,6 +106,8 @@ private void bindControllerCommands() {
         OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
         OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false));
         OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true));
+
+        OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand());
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {

src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java

@@ -7,13 +7,19 @@
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.subsystems.arm.ArmCommands;
+import frc.trigon.robot.subsystems.arm.ArmConstants;
 import frc.trigon.robot.subsystems.climber.ClimberCommands;
 import frc.trigon.robot.subsystems.climber.ClimberConstants;
+import frc.trigon.robot.subsystems.elevator.ElevatorCommands;
+import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
+import frc.trigon.robot.subsystems.intake.IntakeCommands;
+import frc.trigon.robot.subsystems.intake.IntakeConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import lib.hardware.misc.leds.LEDCommands;
 
 public class ClimbCommands {
-    public static boolean IS_CLIMBING = false;
+    public static boolean IS_CLIMBING = false;//TODO: Make score triggers not work while climbing
 
     public static Command getClimbCommand() {//TODO: Set other component positions
         return new SequentialCommandGroup(
@@ -23,11 +29,11 @@ public static Command getClimbCommand() {//TODO: Set other component positions
                 ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB)
                         .until(RobotContainer.CLIMBER::atTargetState),
                 getAdjustClimbManuallyCommand()
-        ).alongWith(getClimbLEDCommand()).finallyDo(() -> IS_CLIMBING = false);
+        ).alongWith(getSetSubsystemsToRestForClimbCommand(), getClimbLEDCommand()).finallyDo(() -> IS_CLIMBING = false);
     }
 
     private static Command getClimbLEDCommand() {
-        return LEDCommands.getAnimateCommand(LEDConstants.CLIMB_ANIMATION_SETTINGS);//TODO: Add LEDStrip
+        return LEDCommands.getAnimateCommand(LEDConstants.CLIMB_ANIMATION_SETTINGS);
     }
 
     private static Command getAdjustClimbManuallyCommand() {
@@ -40,4 +46,12 @@ private static Command getAdjustClimbManuallyCommand() {
                 )
         );
     }
+
+    private static Command getSetSubsystemsToRestForClimbCommand() {
+        return new ParallelCommandGroup(
+                ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST_FOR_CLIMB),
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_FOR_CLIMB),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST_FOR_CLIMB)
+        );
+    }
 }

src/main/java/frc/trigon/robot/constants/OperatorConstants.java

@@ -48,7 +48,8 @@ public class OperatorConstants {
             SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(),
             SCORE_CORAL_LEFT_TRIGGER = DRIVER_CONTROLLER.leftBumper(),
             SCORE_CORAL_RIGHT_TRIGGER = DRIVER_CONTROLLER.rightBumper(),
-            EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e());
+            EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e()),
+            CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c());
 
     public static final Trigger
             SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a()),

src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java

@@ -237,7 +237,7 @@ private static void configureDistanceSensor() {
     public enum ArmState {
         REST(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0),
         REST_WITH_CORAL(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180), 0),
-        REST_FOR_CLIMB(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0),
+        REST_FOR_CLIMB(Rotation2d.fromDegrees(120), Rotation2d.fromDegrees(120), 0),
         LOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4),
         HOLD_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4),
         EJECT(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(60), 4),

src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java

@@ -174,15 +174,16 @@ private static void configureReverseLimitSensor() {
 
     public enum ElevatorState {
         REST(0.603, 0.603, 0.7),
+        REST_WITH_ALGAE(0.603, 0.603, 0.3),
+        REST_FOR_CLIMB(0, 0, 0.5),
         LOAD_CORAL(0.5519, 0.5519, 0.7),
         SCORE_L1(0.1, 0.1, 1),
         SCORE_L2(0.3, 0.4, 1),
         SCORE_L3(0.7, 0.8, 1),
         SCORE_L4(1.2, 1.3, 1),
-        COLLECT_ALGAE_FROM_L2(0.603, 0.603, 1),
-        COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1),
-        COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7),
-        REST_WITH_ALGAE(0.603, 0.603, 0.3),
+        COLLECT_ALGAE_FROM_L2(0.603, 0.603, 0.7),
+        COLLECT_ALGAE_FROM_L3(0.953, 0.953, 0.7),
+        COLLECT_ALGAE_FROM_GROUND(0, 0, 1),
         SCORE_NET(1.382, 1.382, 0.3);
 
         public final double targetPositionMeters;

src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java

@@ -22,7 +22,6 @@
 import lib.hardware.phoenix6.talonfx.TalonFXSignal;
 import lib.hardware.simulation.SimpleMotorSimulation;
 import lib.hardware.simulation.SingleJointedArmSimulation;
-import lib.utilities.Conversions;
 import lib.utilities.mechanisms.SingleJointedArmMechanism2d;
 import lib.utilities.mechanisms.SpeedMechanism2d;
 
@@ -219,6 +218,7 @@ private static void configureDistanceSensor() {
 
     public enum IntakeState {
         REST(0, MINIMUM_ANGLE),
+        REST_FOR_CLIMB(0, MAXIMUM_ANGLE),
         COLLECT(5, MAXIMUM_ANGLE),
         EJECT(-5, MAXIMUM_ANGLE);