Created a Climb command group

Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommand.java

@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ClimbConstants;
+import frc.robot.subsystems.Climb;
+
+// Creates a new Climb command.
+public class ClimbCommand extends Command 
+{
+  private final Climb m_Climb;
+
+  /**
+   * Climb command constructor.
+   * @param climb Climb subsystem.
+   */
+  public ClimbCommand(Climb climb) 
+  {
+    this.m_Climb = climb;
+    addRequirements(m_Climb);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() 
+  {
+    m_Climb.setClimbWristPosition(ClimbConstants.CLIMB_WRIST_POSITION);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() 
+  {
+    // Intentionally Empty.
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) 
+  {
+    // Intentionally Empty.
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() 
+  {
+    return false;
+  }
+}

Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommandGroup.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.FunnelOpenCommand;
+import frc.robot.commands.ClimbCommand;
+import frc.robot.subsystems.Climb;
+
+// Creates a new Climb Command group.
+public class ClimbCommandGroup extends SequentialCommandGroup 
+{
+  private final Climb m_Climb;
+
+  /**
+   * Climb command group constructor.
+   * @param climb Climb subsystem.
+   */
+  public ClimbCommandGroup(Climb climb)
+  {
+    this.m_Climb = climb; 
+    // Schedules Funnel Open command then Climb command sequentially.
+    addCommands(
+      new FunnelOpenCommand(m_Climb),
+      new ClimbCommand(m_Climb)
+    );
+  }
+}

Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelClosedCommand.java

@@ -15,7 +15,7 @@ public class FunnelClosedCommand extends Command
 
   /**
    * Funnel Closed command constructor.
-   * @param coralPlacer Coral Placer subsystem.
+   * @param climb Climb subsystem.
    */
   public FunnelClosedCommand(Climb climb) 
   {

Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelOpenCommand.java

@@ -15,7 +15,7 @@ public class FunnelOpenCommand extends Command
 
   /**
    * Funnel Open command constructor.
-   * @param coralPlacer Coral Placer subsystem.
+   * @param climb Climb subsystem.
    */
   public FunnelOpenCommand(Climb climb) 
   {
@@ -46,8 +46,15 @@ public void end(boolean interrupted)
 
   // Returns true when the command should end.
   @Override
-  public boolean isFinished() 
+  public boolean isFinished()
   {
-    return false;
+    if(m_Climb.getFunnelWristPosition() <= ClimbConstants.CORAL_FUNNEL_SEPARATE_POSITION_UPPER_BOUND && m_Climb.getFunnelWristPosition() >= ClimbConstants.CORAL_FUNNEL_SEPARATE_POSITION_LOWER_BOUND)
+    {
+      return true;
+    }
+    else
+    {
+      return false;
+    }
   }
 }

Big-Bad-Wolf-2025/src/main/java/frc/robot/constants/ClimbConstants.java

@@ -12,12 +12,16 @@
 // Initializing constants for Climb.
 public class ClimbConstants 
 {
-    // Insert wrist feed forward and default position values when determined.
+    // Insert wrist feed forward and position values when determined.
     public static final double CLIMB_DEFAULT_WRIST_POSITION = 0;
+    public static final double CLIMB_WRIST_POSITION = 0;
     public static final double CLIMB_WRIST_FEED_FORWARD = 0;
 
     // (REMOVE_BEFORE_COMP) Insert coral funnel constants when determined.
     public static final double CORAL_FUNNEL_SEPARATE_POSITION = 0;
+    // (REMOVE BEFORE COMP) Upper and Lower bound constants will have a 10-20 difference from default separate position.
+    public static final double CORAL_FUNNEL_SEPARATE_POSITION_UPPER_BOUND = 0;
+    public static final double CORAL_FUNNEL_SEPARATE_POSITION_LOWER_BOUND = 0;
     public static final double CORAL_FUNNEL_CLOSED_POSITION = 0;
 
     // Insert wrist gain values when determined.

Big-Bad-Wolf-2025/src/main/java/frc/robot/subsystems/Climb.java

@@ -19,7 +19,7 @@ public class Climb extends SubsystemBase
   private final TalonFX m_ClimbWristMotorFollower = new TalonFX(Hardware.CLIMB_WRIST_MOTOR_FOLLOWER_ID);
 
   private final TalonFX m_CoralFunnelMotor;
-  
+
   /**
    * Climb subsystem constructor.
    */
@@ -55,6 +55,11 @@ public void setFunnelWristPosition(double funnelPosition)
     m_CoralFunnelMotor.setControl(positionRequest);
   }
 
+  public final double getFunnelWristPosition()
+  {
+    return m_CoralFunnelMotor.getPosition().getValueAsDouble();
+  }
+
   @Override
   public void periodic() 
   {