Made Intake Subsystem, can intake and outtake fuel, can move intake up and down.

2026-2706-Robot-Code/src/main/java/frc/robot/Constants.java

@@ -16,7 +16,24 @@ public final class Constants {
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
   }
-  public static class RobotConstants {
+   public static class RobotConstants {
     public static final int kSelectorSwitchPort = 0;
+    public static final int kRPMConversionFactor = 1 / 3;
+
+    public static final int kIntakeMotorID = 15;
+    public static final int kIntakeUpDownMotorID = 30;
+    public static final double kIntakeSpeed = 0.5;
+    public static final double kUpPosition = 20;
+    public static final double kDownPosition = -20; //must be a negitive
+    public static final double kTolerance = 0.3;
+
+    public static final double kUpDownP = 0.2;
+    public static final double kUpDownI = 0.0;
+    public static final double kUpDownD = 0.0; 
+
   }
+
+  public static class UtilityConstants {
+    public static final boolean debugMode = true;
+}
 }

2026-2706-Robot-Code/src/main/java/frc/robot/RobotContainer.java

@@ -7,16 +7,23 @@
 import frc.robot.Constants.OperatorConstants;
 
 import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 
 
+import frc.robot.commands.IntakeDownCommand;
+import frc.robot.commands.IntakeUpCommand;
+import frc.robot.commands.RunIntakeCommandForward;
+import frc.robot.commands.RunIntakeCommandReversed;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.AutoSelectorKnobSubsystem;
 
+import frc.robot.subsystems.IntakeSubsystem;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -25,42 +32,47 @@
  * subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+
+  // Subsystem
+  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
   private final AutoSelectorKnobSubsystem m_AutoSelectorKnobSubsystem = new AutoSelectorKnobSubsystem();
 
-  // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+  // Controller
+  private final XboxController driverController = new XboxController(0);
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    // Configure the trigger bindings
-    configureBindings();
+      configureButtonBindings();
   }
 
-  /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
-   * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-   * joysticks}.
-   */
-  private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
-
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
-  }
+  private void configureButtonBindings() {
+
+        // Toggle intake ON/OFF
+        JoystickButton intakeToggleButton =
+            new JoystickButton(driverController, XboxController.Button.kA.value);
+
+        // Run intake in reverse while held
+        JoystickButton reverseButton =
+            new JoystickButton(driverController, XboxController.Button.kB.value);
 
-
+        // Toggle intake to go down
+        JoystickButton intakeDownButton =
+            new JoystickButton(driverController, XboxController.Button.kX.value);
 
-  /** This function returns the autonomous command based on the knob position. */
+        // Toggle intake to go up
+        JoystickButton intakeUpButton =
+            new JoystickButton(driverController, XboxController.Button.kY.value);
+
+        intakeDownButton.toggleOnTrue(new IntakeDownCommand(intakeSubsystem));
+        intakeUpButton.toggleOnTrue(new IntakeUpCommand(intakeSubsystem));
+        intakeToggleButton.toggleOnTrue(new RunIntakeCommandForward(intakeSubsystem));
+        reverseButton.whileTrue(new RunIntakeCommandReversed(intakeSubsystem));
+    }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
   public Command getAutonomousCommand() {
     int mode = m_AutoSelectorKnobSubsystem.getAutoMode();
     System.out.println("Auto Mode = " + mode); // debug print
@@ -103,5 +115,6 @@ public Command getAutonomousCommand() {
         //null;
       default:
         return null;
-    }}}
-
+    }
+  }
+}

2026-2706-Robot-Code/src/main/java/frc/robot/commands/IntakeDownCommand.java

@@ -0,0 +1,24 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class IntakeDownCommand extends Command {
+
+    private final IntakeSubsystem intake;
+
+    public IntakeDownCommand(IntakeSubsystem intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.moveIntakeDown();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

2026-2706-Robot-Code/src/main/java/frc/robot/commands/IntakeUpCommand.java

@@ -0,0 +1,24 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class IntakeUpCommand extends Command {
+
+    private final IntakeSubsystem intake;
+
+    public IntakeUpCommand(IntakeSubsystem intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.moveIntakeUp();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+}

2026-2706-Robot-Code/src/main/java/frc/robot/commands/RunIntakeCommandForward.java

@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class RunIntakeCommandForward extends Command {
+
+    private final IntakeSubsystem intake;
+
+    public RunIntakeCommandForward(IntakeSubsystem intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.startIntake();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.stopIntake();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

2026-2706-Robot-Code/src/main/java/frc/robot/commands/RunIntakeCommandReversed.java

@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class RunIntakeCommandReversed extends Command {
+
+    private final IntakeSubsystem intake;
+
+    public RunIntakeCommandReversed(IntakeSubsystem intake) {
+        this.intake = intake;
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.reverseIntake();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.stopIntake();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

2026-2706-Robot-Code/src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -0,0 +1,138 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SoftLimitConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.ResetMode;
+import com.revrobotics.PersistMode;
+import com.revrobotics.spark.SparkSoftLimit.SoftLimitDirection;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import frc.robot.Constants;
+import frc.robot.Constants.*;
+
+
+
+/**
+ * The Intake class represents the intake subsystem of the robot.
+ * It controls the intake motor and provides methods to set the motor power,
+ * stop the motor, and get the motor's RPM.
+ */
+public class IntakeSubsystem extends SubsystemBase {
+    private SparkMax intakeMotor;
+    private SparkMax intakeUpDownMotor;
+    private final RelativeEncoder intakeUpDownEncoder;
+    private final SparkClosedLoopController intakeUpDownPID;
+
+    /**
+     * Constructs a new Intake subsystem.
+     * Initializes the intake motor.
+     */
+    public IntakeSubsystem() {
+        intakeMotor = new SparkMax(Constants.RobotConstants.kIntakeMotorID, MotorType.kBrushless);
+
+        intakeUpDownMotor = new SparkMax(Constants.RobotConstants.kIntakeUpDownMotorID, MotorType.kBrushless);
+        intakeUpDownEncoder = intakeUpDownMotor.getEncoder();
+        intakeUpDownPID = intakeUpDownMotor.getClosedLoopController();
+
+        // Resets the position to 0, turn the robot off while in up position to prevent things breaking.
+        intakeUpDownEncoder.setPosition(0.0);
+
+        SparkMaxConfig upDownConfig = new SparkMaxConfig();
+            upDownConfig.closedLoop
+                .p(RobotConstants.kUpDownP)
+                .i(RobotConstants.kUpDownI)
+                .d(RobotConstants.kUpDownD);
+
+            upDownConfig.softLimit
+                .forwardSoftLimit(RobotConstants.kUpPosition)
+                .forwardSoftLimitEnabled(true)
+                .reverseSoftLimit(RobotConstants.kDownPosition)
+                .reverseSoftLimitEnabled(true);
+            intakeUpDownMotor.configure(upDownConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    /**
+     * Starts the intake motor.
+     */
+    public void startIntake() {
+        intakeMotor.set(RobotConstants.kIntakeSpeed);
+
+    }
+
+    /**
+     * Starts the intake motor, but backwards.
+     */
+    public void reverseIntake() {
+        intakeMotor.set(-RobotConstants.kIntakeSpeed);
+
+    }
+
+    /**
+     * Stops the intake motor.
+     */
+    public void stopIntake(){
+
+        intakeMotor.set(0);
+
+    }
+
+    /**
+     * Gets the raw RPM of the intake motor.
+     * 
+     * @return The raw RPM of the intake motor.
+     */
+    public double getRawMotorRPM(){
+
+        return intakeMotor.getEncoder().getVelocity();
+
+    }
+
+    /**
+     * Gets the RPM of the intake motor, converted using a conversion factor.
+     * 
+     * @return The converted RPM of the intake motor.
+     */
+    /*public double getRPM(){
+
+        return getRawMotorRPM() * RobotConstants.kRPMConversionFactor;
+
+    }*/
+
+    /**
+     * Moves the intake down.
+     */
+    public void moveIntakeDown() {
+    intakeUpDownPID.setSetpoint(RobotConstants.kDownPosition, ControlType.kPosition);
+}
+
+    /**
+     * Moves the intake up.
+     */
+    public void moveIntakeUp() {
+    intakeUpDownPID.setSetpoint(RobotConstants.kUpPosition, ControlType.kPosition);
+}
+
+    /**
+     * Periodically updates the SmartDashboard with the intake motor's RPM and current.
+     * This method is called automatically to update sensor status on the dashboard.
+     */
+    @Override
+    public void periodic() {
+        if (UtilityConstants.debugMode){
+            SmartDashboard.putNumber("Intake RPM", getRawMotorRPM());
+            SmartDashboard.putNumber("Intake Current", intakeMotor.getOutputCurrent());
+        }
+    }
+
+}