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..6a352c0 --- /dev/null +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -0,0 +1,7 @@ +package frc.robot.constants; + +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 new file mode 100644 index 0000000..06420f2 --- /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..84d6fa2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber/Climber.java @@ -0,0 +1,89 @@ +package frc.robot.subsystems.Climber; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.states.ClimberState; +import frc.robot.util.PARTs.Classes.PARTsCommandUtils; +import frc.robot.util.PARTs.Classes.Abstracts.PARTsSubsystem; + +public abstract class Climber extends PARTsSubsystem { + private ClimberState climberState = ClimberState.IDLE; + + public Climber() { + super("Climber"); + } + + // 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 + + @Override + public void periodic() { + 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. + * + * @param speed The speed between -1.0 and 1.0. + */ + protected abstract void setSpeed(double speed); + + public ClimberState getState() { + return climberState; + } + + + public Command idle() { + return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { + 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 +} 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..7da3a59 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber/ClimberPhys.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.Climber; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.constants.ClimberConstants; + +public class ClimberPhys extends Climber { + protected final SparkMax climberMotor; + protected final RelativeEncoder climberEncoder; + + public ClimberPhys() { + super(); + + SparkMaxConfig climberConfig = new SparkMaxConfig(); + climberConfig.idleMode(IdleMode.kBrake); + + 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) { + 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 new file mode 100644 index 0000000..8df0907 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber/ClimberSim.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.Climber; + +public class ClimberSim extends Climber { + @Override + protected void setSpeed(double speed) { + partsNT.putDouble("Our Climber Speed", speed); + } +}