From c5bb959bfa66e32707628a1fee2bc7fe80333638 Mon Sep 17 00:00:00 2001 From: Danielle Sakai Date: Sat, 25 Jan 2025 15:18:55 -0800 Subject: [PATCH 1/3] climb command initial commit --- .../java/frc/robot/commands/ClimbCommand.java | 53 +++++++++++++++++++ .../frc/robot/commands/ClimbCommandGroup.java | 30 +++++++++++ .../robot/commands/FunnelClosedCommand.java | 2 +- .../frc/robot/commands/FunnelOpenCommand.java | 2 +- .../frc/robot/constants/ClimbConstants.java | 3 +- 5 files changed, 87 insertions(+), 3 deletions(-) create mode 100644 Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommand.java create mode 100644 Big-Bad-Wolf-2025/src/main/java/frc/robot/commands/ClimbCommandGroup.java 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..4ca9f65 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) { 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..51fb927 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,8 +12,9 @@ // 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. From 5d04dac098681de7e22a8d9342ac53fc86fff8e0 Mon Sep 17 00:00:00 2001 From: Danielle Sakai Date: Tue, 28 Jan 2025 20:12:34 -0800 Subject: [PATCH 2/3] Rewrote isFinished method in FunnelOpenCommand. --- .../java/frc/robot/commands/FunnelOpenCommand.java | 11 +++++++++-- .../src/main/java/frc/robot/subsystems/Climb.java | 7 ++++++- 2 files changed, 15 insertions(+), 3 deletions(-) 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 4ca9f65..bf8b436 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 @@ -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) + { + return true; + } + else + { + return false; + } } } 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() { From 97af080db76d91ba3a31e057cb3625b870558e42 Mon Sep 17 00:00:00 2001 From: Danielle Sakai Date: Sat, 1 Feb 2025 10:10:26 -0800 Subject: [PATCH 3/3] Added an upper and lower bound to the Funnel Open isFinished method. --- .../src/main/java/frc/robot/commands/FunnelOpenCommand.java | 2 +- .../src/main/java/frc/robot/constants/ClimbConstants.java | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) 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 bf8b436..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 @@ -48,7 +48,7 @@ public void end(boolean interrupted) @Override public boolean isFinished() { - if(m_Climb.getFunnelWristPosition() == ClimbConstants.CORAL_FUNNEL_SEPARATE_POSITION) + if(m_Climb.getFunnelWristPosition() <= ClimbConstants.CORAL_FUNNEL_SEPARATE_POSITION_UPPER_BOUND && m_Climb.getFunnelWristPosition() >= ClimbConstants.CORAL_FUNNEL_SEPARATE_POSITION_LOWER_BOUND) { return true; } 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 51fb927..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 @@ -19,6 +19,9 @@ public class ClimbConstants // (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.