diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 1c6c0dc..7675772 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -41,6 +41,7 @@ public class Controls { public Controls(Subsystems subsystems) { s = subsystems; + configureIntake(); // Configure the trigger bindings configureDrivebaseBindings(); } @@ -81,6 +82,12 @@ private double getDriveRotate() { .withRotationalDeadband(SWERVE_DEADBAND) .withDriveRequestType(DriveRequestType.Velocity); + private void configureIntake() { + if (s.intakeSubsystem != null) { + driverController.rightBumper().whileTrue(s.intakeSubsystem.startIntake()); + } + } + private void configureDrivebaseBindings() { if (s.drivebaseSubsystem == null) { // Stop running this method diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java index bf233a5..81b53c4 100644 --- a/src/main/java/frc/robot/Hardware.java +++ b/src/main/java/frc/robot/Hardware.java @@ -7,4 +7,7 @@ public final class Hardware { public static final int PDH_ID = 1; + + // TODO: better number when get robot + public static final int IntakeMotor = 0; } diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index 71f4545..c20d12d 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -1,19 +1,23 @@ package frc.robot; import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.INTAKE_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; public class Subsystems { // _ENABLED constants go here public static class SubsystemConstants { public static final boolean DRIVEBASE_ENABLED = true; + public static final boolean INTAKE_ENABLED = true; } public final CommandSwerveDrivetrain drivebaseSubsystem; + public final IntakeSubsystem intakeSubsystem; public Subsystems(Mechanism2d mechanism2d) { // Initialize subsystems here (don't forget to check if they're enabled!) @@ -33,5 +37,11 @@ public Subsystems(Mechanism2d mechanism2d) { } else { drivebaseSubsystem = null; } + + if (INTAKE_ENABLED) { + intakeSubsystem = new IntakeSubsystem(); + } else { + intakeSubsystem = null; + } } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java new file mode 100644 index 0000000..1ea98e4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -0,0 +1,72 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Hardware; + +public class IntakeSubsystem extends SubsystemBase { + private final VelocityTorqueCurrentFOC velocityRequest = new VelocityTorqueCurrentFOC(0); + + private final TalonFX intakeMotor; + + // TODO: set rps + private static final double TARGET_RPS = 40.0; + + public IntakeSubsystem() { + // TODO: check with mech if motor on canivore or roborio + intakeMotor = new TalonFX(Hardware.IntakeMotor); + motorConfigs(); + } + + private void motorConfigs() { + var talonFXConfigs = new TalonFXConfiguration(); + talonFXConfigs.MotorOutput.NeutralMode = NeutralModeValue.Coast; // KEEP THIS AT COAST + talonFXConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + talonFXConfigs.CurrentLimits.StatorCurrentLimit = 80; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = 40; + talonFXConfigs.CurrentLimits.StatorCurrentLimitEnable = true; + talonFXConfigs.CurrentLimits.SupplyCurrentLimitEnable = true; + + // TODO: set values + talonFXConfigs.Slot0.kP = 0.0; + talonFXConfigs.Slot0.kS = 0.0; + talonFXConfigs.Slot0.kA = 0.0; + + // configurator + StatusCode status = StatusCode.StatusCodeNotInitialized; + for (int i = 0; i < 5; ++i) { + status = intakeMotor.getConfigurator().apply(talonFXConfigs); + if (status.isOK()) break; + } + if (!status.isOK()) { + DriverStation.reportError("Failed to apply IntakeMotor configs: " + status, false); + } + } + + public Command startIntake() { + return startEnd( + () -> { + runIntake(TARGET_RPS); + }, + () -> { + stopIntake(); + }) + .withName("Run Intake"); + } + + private void runIntake(double velocity) { + intakeMotor.setControl(velocityRequest.withVelocity(velocity)); + } + + private void stopIntake() { + intakeMotor.stopMotor(); + } +}