diff --git a/src/main/java/frc/lib/lib2706/SubsystemChecker.java b/src/main/java/frc/lib/lib2706/SubsystemChecker.java index e1e59499..4166cd37 100644 --- a/src/main/java/frc/lib/lib2706/SubsystemChecker.java +++ b/src/main/java/frc/lib/lib2706/SubsystemChecker.java @@ -40,6 +40,7 @@ public enum SubsystemType { SubsystemType.VisionNTSubsystem, SubsystemType.GripperSubsystem, SubsystemType.BlingSubsystem, + SubsystemType.ClimberSubsystem, }; // RobotID: 1, 2022 robot, RapidReact, Clutch @@ -60,6 +61,7 @@ public enum SubsystemType { SubsystemType.DiffTalonSubsystem, // Chassis SubsystemType.RelaySubsystem, SubsystemType.BlingSubsystem, + SubsystemType.ClimberSubsystem, }; // RobotID: 3, 2019 Comp Robot, Deep Space, Mergonaut diff --git a/src/main/java/frc/robot/commands/ClimberRPM.java b/src/main/java/frc/robot/commands/ClimberRPM.java index 5adeb576..18a13603 100644 --- a/src/main/java/frc/robot/commands/ClimberRPM.java +++ b/src/main/java/frc/robot/commands/ClimberRPM.java @@ -4,24 +4,21 @@ package frc.robot.commands; import java.util.function.DoubleSupplier; -import java.util.function.Supplier; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ClimberSubsystem; -public class ClimberRPM extends CommandBase { +public class ClimberRPM extends Command { private ClimberSubsystem climber; private DoubleSupplier m_getPercentOutput; /** Creates a new IndexerCargo. */ public ClimberRPM(DoubleSupplier getPercentOutput) { - + climber = ClimberSubsystem.getInstance(); - - // Use addRequirements() here to declare subsystem dependencies. - if ( climber != null ) - { - addRequirements(climber); + + if (climber != null){ + addRequirements(climber); } m_getPercentOutput = getPercentOutput; diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 5da8d036..b7756920 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -9,32 +9,33 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkLowLevel.MotorType; +import frc.lib.lib2706.SubsystemChecker; +import frc.lib.lib2706.SubsystemChecker.SubsystemType; import frc.robot.Config; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ClimberSubsystem extends SubsystemBase { //Instance Variables + private static ClimberSubsystem instance = null; // static object that contains all movement controls private CANSparkMax m_climber; private RelativeEncoder m_encoder; - //private double targetRPM = CLIMBER_RPM.getValue(); public double kMaxOutput = 1; public double kMinOutput = -1; public double currentPosition = 0; public boolean m_bGoodSensors = false; - private static final ClimberSubsystem INSTANCE_CLIMBER = new ClimberSubsystem(); + + public static ClimberSubsystem getInstance() { + if (instance == null) { + SubsystemChecker.subsystemConstructed(SubsystemType.ClimberSubsystem); + instance = new ClimberSubsystem(); + } + return instance; + } /** Creates a new ClimberSubSystem. */ private ClimberSubsystem() { - - if (Config.Climber_CANID.CLIMBER != -1) { - initializeSubsystem(); - } - else - { - m_climber = null; - } - + initializeSubsystem(); } private void initializeSubsystem() @@ -59,16 +60,6 @@ public boolean isAvailable() return m_climber != null; } - /* - * Returns the singleton instance for the Climber Subsystem - */ - public static ClimberSubsystem getInstance() { - if ( INSTANCE_CLIMBER.isAvailable() == true) - return INSTANCE_CLIMBER; - else - return null; - } - //Run the climber public void StartClimberRPM(double percentOutput){