From f5e1beb497fa64a6a7c379da46b11003c260866e Mon Sep 17 00:00:00 2001 From: AlainahH Date: Mon, 19 Jan 2026 12:17:33 -0500 Subject: [PATCH 1/2] initial climber things --- .../frc/robot/constants/ClimberConstants.java | 6 ++ .../java/frc/robot/states/ClimberState.java | 21 +++++ .../frc/robot/subsystems/Climber/Climber.java | 85 +++++++++++++++++++ .../robot/subsystems/Climber/ClimberPhys.java | 46 ++++++++++ .../robot/subsystems/Climber/ClimberSim.java | 10 +++ 5 files changed, 168 insertions(+) create mode 100644 src/main/java/frc/robot/constants/ClimberConstants.java create mode 100644 src/main/java/frc/robot/states/ClimberState.java create mode 100644 src/main/java/frc/robot/subsystems/Climber/Climber.java create mode 100644 src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java create mode 100644 src/main/java/frc/robot/subsystems/Climber/ClimberSim.java diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java new file mode 100644 index 0000000..3f1b2ec --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class ClimberConstants { + public static final double CLIMBING_SPEED = .1; + public static final double DECLIMBING_SPEED = -.1; +} diff --git a/src/main/java/frc/robot/states/ClimberState.java b/src/main/java/frc/robot/states/ClimberState.java new file mode 100644 index 0000000..38d22cd --- /dev/null +++ b/src/main/java/frc/robot/states/ClimberState.java @@ -0,0 +1,21 @@ +package frc.robot.states; + +import frc.robot.constants.ClimberConstants; + +/*States the Climber can be in */ +public enum ClimberState { + IDLE(0), + DISABLED(0), + CLIMBING(ClimberConstants.CLIMBING_SPEED), + DECLIMB(ClimberConstants.DECLIMBING_SPEED); + + private final double speed; + + ClimberState(double speed) { + this.speed = speed; + } + + public double getSpeed() { + return speed; + } +} diff --git a/src/main/java/frc/robot/subsystems/Climber/Climber.java b/src/main/java/frc/robot/subsystems/Climber/Climber.java new file mode 100644 index 0000000..61b2dcc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber/Climber.java @@ -0,0 +1,85 @@ +package frc.robot.subsystems.Climber; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConstants; +import frc.robot.states.ClimberState; +import frc.robot.util.PARTs.Classes.PARTsCommandUtils; +import frc.robot.states.ShooterState; +import frc.robot.util.PARTs.Classes.Abstracts.PARTsSubsystem; + +public abstract class Climber extends PARTsSubsystem{ + private ClimberState ClimberState = ClimberState.IDLE; + + public Climber() { + super("Climber"); + if (RobotConstants.DEBUGGING) { + partsNT.putDouble("Climber Speed", 0); + } + } + + //region Generic Subsystem Functions + @Override + public void outputTelemetry() { + } + + @Override + public void stop() { + } + + @Override + public void reset() { + } + + @Override + public void log() { + } + //endregion + + @Override + public void periodic() { + if (RobotConstants.DEBUGGING) { + setSpeed(partsNT.getDouble("Climber Speed")); + } + else { + switch (shooterState) { + case CHARGING: + setSpeed(shooterState.getSpeed()); + break; + case DISABLED: + setSpeed(shooterState.getSpeed()); + break; + case IDLE: + setSpeed(shooterState.getSpeed()); + break; + case SHOOTING: + setSpeed(shooterState.getSpeed()); + break; + default: + setSpeed(0); + break; + + } + } + } + + //region Custom Public Functions + /** Sets the speed of the Shooter. + * @param speed The speed between -1.0 and 1.0. + */ + protected abstract void setSpeed(double speed); + + public ShooterState getState() { return shooterState; } + + public Command shoot() { + return PARTsCommandUtils.setCommandName("Command Shoot", this.runOnce(() -> { + shooterState = ShooterState.SHOOTING; + })); + } + + public Command idle() { + return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { + shooterState = ShooterState.IDLE; + })); + } + //endregion +} diff --git a/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java b/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java new file mode 100644 index 0000000..f2f46cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.Shooter; + +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.constants.ShooterConstants; + +public class ShooterPhys extends Shooter { + protected final SparkMax rightMotor; + protected final SparkMax leftMotor; + + protected final RelativeEncoder leftEncoder; + protected final RelativeEncoder rightEncoder; + + public ShooterPhys() { + super(); + + SparkMaxConfig shooterConfig = new SparkMaxConfig(); + shooterConfig.idleMode(IdleMode.kCoast); + + leftMotor = new SparkMax(ShooterConstants.LEFT_MOTOR_ID, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); + leftEncoder = leftMotor.getEncoder(); + leftMotor.configure(shooterConfig, com.revrobotics.ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + + rightMotor = new SparkMax(ShooterConstants.RIGHT_MOTOR_ID, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); + rightEncoder = rightMotor.getEncoder(); + rightMotor.configure(shooterConfig.follow(leftMotor, true), com.revrobotics.ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + } + + @Override + protected void setSpeed(double speed) { + leftMotor.set(speed); + } + + @Override + public void periodic() { + super.periodic(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java b/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java new file mode 100644 index 0000000..258e661 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.Shooter; + +public class ShooterSim extends Shooter { + + @Override + protected void setSpeed(double speed) { + partsNT.putDouble("Our Shooter Speed", speed); + } + +} From 6002cd4cbf2de2ce06391f2ceee29c7f2db5fcaa Mon Sep 17 00:00:00 2001 From: AlainahH Date: Mon, 19 Jan 2026 13:42:00 -0500 Subject: [PATCH 2/2] Made everything for the Climber --- .../frc/robot/constants/ClimberConstants.java | 1 + .../java/frc/robot/states/ClimberState.java | 2 +- .../frc/robot/subsystems/Climber/Climber.java | 88 ++++++++++--------- .../robot/subsystems/Climber/ClimberPhys.java | 49 ++++++----- .../robot/subsystems/Climber/ClimberSim.java | 8 +- 5 files changed, 77 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index 3f1b2ec..6a352c0 100644 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -3,4 +3,5 @@ public class ClimberConstants { public static final double CLIMBING_SPEED = .1; public static final double DECLIMBING_SPEED = -.1; + public static final int CLIMBER_MOTOR_ID = 0; } diff --git a/src/main/java/frc/robot/states/ClimberState.java b/src/main/java/frc/robot/states/ClimberState.java index 38d22cd..06420f2 100644 --- a/src/main/java/frc/robot/states/ClimberState.java +++ b/src/main/java/frc/robot/states/ClimberState.java @@ -2,7 +2,7 @@ import frc.robot.constants.ClimberConstants; -/*States the Climber can be in */ +/* States the Climber can be in */ public enum ClimberState { IDLE(0), DISABLED(0), diff --git a/src/main/java/frc/robot/subsystems/Climber/Climber.java b/src/main/java/frc/robot/subsystems/Climber/Climber.java index 61b2dcc..84d6fa2 100644 --- a/src/main/java/frc/robot/subsystems/Climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber/Climber.java @@ -1,85 +1,89 @@ package frc.robot.subsystems.Climber; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConstants; import frc.robot.states.ClimberState; import frc.robot.util.PARTs.Classes.PARTsCommandUtils; -import frc.robot.states.ShooterState; import frc.robot.util.PARTs.Classes.Abstracts.PARTsSubsystem; -public abstract class Climber extends PARTsSubsystem{ - private ClimberState ClimberState = ClimberState.IDLE; +public abstract class Climber extends PARTsSubsystem { + private ClimberState climberState = ClimberState.IDLE; public Climber() { super("Climber"); - if (RobotConstants.DEBUGGING) { - partsNT.putDouble("Climber Speed", 0); - } } - //region Generic Subsystem Functions + // region Generic Subsystem Functions @Override public void outputTelemetry() { + partsNT.putString("Climber State", climberState.toString()); } @Override public void stop() { + climberState = climberState.DISABLED; } @Override public void reset() { + climberState = climberState.IDLE; } @Override public void log() { + partsLogger.logString("Climber State", climberState.toString()); } - //endregion + // endregion @Override public void periodic() { - if (RobotConstants.DEBUGGING) { - setSpeed(partsNT.getDouble("Climber Speed")); - } - else { - switch (shooterState) { - case CHARGING: - setSpeed(shooterState.getSpeed()); - break; - case DISABLED: - setSpeed(shooterState.getSpeed()); - break; - case IDLE: - setSpeed(shooterState.getSpeed()); - break; - case SHOOTING: - setSpeed(shooterState.getSpeed()); - break; - default: - setSpeed(0); - break; - - } + switch (climberState) { + case CLIMBING: + setSpeed(climberState.getSpeed()); + break; + case DECLIMB: + setSpeed(climberState.getSpeed()); + break; + case DISABLED: + setSpeed(climberState.getSpeed()); + break; + case IDLE: + setSpeed(climberState.getSpeed()); + break; + default: + setSpeed(0); + break; } } - //region Custom Public Functions - /** Sets the speed of the Shooter. + // region Custom Public Functions + /** + * Sets the speed of the Shooter. + * * @param speed The speed between -1.0 and 1.0. - */ + */ protected abstract void setSpeed(double speed); - public ShooterState getState() { return shooterState; } - - public Command shoot() { - return PARTsCommandUtils.setCommandName("Command Shoot", this.runOnce(() -> { - shooterState = ShooterState.SHOOTING; - })); + public ClimberState getState() { + return climberState; } + public Command idle() { return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { - shooterState = ShooterState.IDLE; + climberState = ClimberState.IDLE; + })); + } + + public Command climb() { + return PARTsCommandUtils.setCommandName("Command Climb", this.runOnce(() -> { + climberState = ClimberState.CLIMBING; + })); + } + + public Command declimb() { + return PARTsCommandUtils.setCommandName("Command Declimb", this.runOnce(() -> { + climberState = ClimberState.DECLIMB; })); } - //endregion + // endregion } diff --git a/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java b/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java index f2f46cc..7da3a59 100644 --- a/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java +++ b/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java @@ -1,46 +1,49 @@ -package frc.robot.subsystems.Shooter; +package frc.robot.subsystems.Climber; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import frc.robot.constants.ShooterConstants; +import frc.robot.constants.ClimberConstants; -public class ShooterPhys extends Shooter { - protected final SparkMax rightMotor; - protected final SparkMax leftMotor; +public class ClimberPhys extends Climber { + protected final SparkMax climberMotor; + protected final RelativeEncoder climberEncoder; - protected final RelativeEncoder leftEncoder; - protected final RelativeEncoder rightEncoder; - - public ShooterPhys() { + public ClimberPhys() { super(); - SparkMaxConfig shooterConfig = new SparkMaxConfig(); - shooterConfig.idleMode(IdleMode.kCoast); - - leftMotor = new SparkMax(ShooterConstants.LEFT_MOTOR_ID, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); - leftEncoder = leftMotor.getEncoder(); - leftMotor.configure(shooterConfig, com.revrobotics.ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); + SparkMaxConfig climberConfig = new SparkMaxConfig(); + climberConfig.idleMode(IdleMode.kBrake); - rightMotor = new SparkMax(ShooterConstants.RIGHT_MOTOR_ID, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); - rightEncoder = rightMotor.getEncoder(); - rightMotor.configure(shooterConfig.follow(leftMotor, true), com.revrobotics.ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); + climberMotor = new SparkMax(ClimberConstants.CLIMBER_MOTOR_ID, MotorType.kBrushless); + climberEncoder = climberMotor.getEncoder(); + climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @Override protected void setSpeed(double speed) { - leftMotor.set(speed); + climberMotor.set(speed); } @Override public void periodic() { super.periodic(); } + + @Override + public void outputTelemetry() { + partsNT.putDouble("Output", climberMotor.getOutputCurrent()); + partsNT.putDouble("Current", climberMotor.getAppliedOutput()); + } + + @Override + public void log() { + partsLogger.logDouble("Output", climberMotor.getOutputCurrent()); + partsLogger.logDouble("Current", climberMotor.getAppliedOutput()); + } } diff --git a/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java b/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java index 258e661..8df0907 100644 --- a/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java +++ b/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java @@ -1,10 +1,8 @@ -package frc.robot.subsystems.Shooter; - -public class ShooterSim extends Shooter { +package frc.robot.subsystems.Climber; +public class ClimberSim extends Climber { @Override protected void setSpeed(double speed) { - partsNT.putDouble("Our Shooter Speed", speed); + partsNT.putDouble("Our Climber Speed", speed); } - }