diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index c20d12d..b22ef52 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -2,11 +2,13 @@ import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; import static frc.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.CLIMB_ENABLED; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import frc.robot.generated.AlphaTunerConstants; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.ClimbSubsystem; public class Subsystems { @@ -14,6 +16,7 @@ public class Subsystems { public static class SubsystemConstants { public static final boolean DRIVEBASE_ENABLED = true; public static final boolean INTAKE_ENABLED = true; + public static final boolean CLIMB_ENABLED = true; } public final CommandSwerveDrivetrain drivebaseSubsystem; @@ -43,5 +46,12 @@ public Subsystems(Mechanism2d mechanism2d) { } else { intakeSubsystem = null; } + + if (CLIMB_ENABLED) { + climbSubsystem = new ClimbSubsystem(); + + } else { + climbSubsystem = null; + } } } diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java new file mode 100644 index 0000000..fc1dd92 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -0,0 +1,83 @@ +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Hardware; + +public class ClimbSubsystem extends SubsystemBase { + public final int CLIMB_MOTOR_ID = 0; // TODO: placeholder + + private final TalonFX climbMotor; + + private double targetPos; + public final double TARGET_POS_START = 0.0; + public final double TARGET_POS_END = 0.0; + + // TODO: private final FlywheelSim motorSim; // TODO: add sim class later + + public ClimbSubsystem() { + climbMotor = new TalonFX(CLIMB_MOTOR_ID); + climbConfig(); + if (RobotBase.isSimulation()) { + // TODO: add sim later + // motorSim = null; + } else { + // motorSim = null; + } + } + + public void climbConfig() { + + TalonFXConfigurator cfg = climbMotor.getConfigurator(); + TalonFXConfiguration talonFXConfiguration = new TalonFXConfiguration(); + + talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast; + + // enabling current limits + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 50; + talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 30; + talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; + talonFXConfiguration.CurrentLimits.SupplyCurrentLowerLimit = 0; + talonFXConfiguration.Slot0.kA = 0.5; + talonFXConfiguration.Slot0.kS = 2.2; + talonFXConfiguration.Slot0.kP = 10; + + StatusCode status = StatusCode.StatusCodeNotInitialized; + for (int i = 0; i < 5; ++i) { + status = climbMotor.getConfigurator().apply(talonFXConfigs); + if (status.isOK()) break; + } + if (!status.isOK()) { + DriverStation.reportError("Failed to apply ClimbMotor configs: " + status, false); + } + } + + public Command setPos(double pos) { + return runOnce( + () -> { + climbMotor.setPosition(pos); + targetPos = pos; + }); + } + + public void stopMotor() { + climbMotor.stopMotor(); + } + + @Override + public void simulationPeriodic() { + // TODO: add sim periodic + // motorSim.setInput(climbMotor.getSimState().getMotorVoltage()); + // motorSim.update(TimedRobot.kDefaultPeriod); // every 20 ms + } + + @Override + public void periodic() { + // TODO: add stuff later + } +}