diff --git a/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommand.java b/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommand.java new file mode 100644 index 0000000..7563319 --- /dev/null +++ b/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; + } +} diff --git a/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommandGroup.java b/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommandGroup.java new file mode 100644 index 0000000..3c42560 --- /dev/null +++ b/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) + ); + } +} \ No newline at end of file diff --git a/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelClosedCommand.java b/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelClosedCommand.java index 120131c..7c2cbfd 100644 --- a/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelClosedCommand.java +++ b/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) { diff --git a/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelOpenCommand.java b/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelOpenCommand.java index 720ac58..f5d5318 100644 --- a/Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/FunnelOpenCommand.java +++ b/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; + } } } diff --git a/Big-Bad-Wolf-2025/src/main/java/frc/robot/constants/ClimbConstants.java b/Big-Bad-Wolf-2025/src/main/java/frc/robot/constants/ClimbConstants.java index c1e16fa..eb61b2c 100644 --- a/Big-Bad-Wolf-2025/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/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. diff --git a/Big-Bad-Wolf-2025/src/main/java/frc/robot/subsystems/Climb.java b/Big-Bad-Wolf-2025/src/main/java/frc/robot/subsystems/Climb.java index 1ccdf3f..8a466e2 100644 --- a/Big-Bad-Wolf-2025/src/main/java/frc/robot/subsystems/Climb.java +++ b/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() {