FRC4048 · LoM-333 · Dec 2, 2025 · Dec 9, 2025
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -0,0 +1,97 @@
+{
+  "Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -47,11 +47,14 @@ public enum Mode {
     public static final boolean DEBUG = true;
     public static final int ROLLER_MOTOR_ID = 1;
     public static final int TILT_MOTOR_ID = 2;
+    public static final int INTAKE_MOTOR_ID = 3;
 
     public static final double ROLLER_SPEED = 0.25;
     public static final double TILT_SPEED = -0.5; // Arm motor is inverted - use negative speed
+    public static final double INTAKE_SPEED = 0.5;
     public static final double SPIN_TIMEOUT = 5;
     public static final double TILT_TIMEOUT = 5;
+    public static final double INTAKE_TIMEOUT = 4;
     public static final double TILT_LENGTH = 0.2;
     public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45);
     public static final Rotation2d TILT_MAX_ANGLE = Rotation2d.fromDegrees(90);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,9 +8,12 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.intake.FlushIntake;
+import frc.robot.commands.intake.SpinIntake;
 import frc.robot.commands.roller.SpinRoller;
 import frc.robot.commands.tilt.TiltDown;
 import frc.robot.commands.tilt.TiltUp;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.RollerSubsystem;
 import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.utils.simulation.RobotVisualizer;
@@ -25,6 +28,7 @@ public class RobotContainer {
     // The robot's subsystems and commands are defined here...
     private final RollerSubsystem rollerSubsystem;
     private final TiltSubsystem tiltSubsystem;
+    private final IntakeSubsystem intakeSubsystem;
 
     private RobotVisualizer robotVisualizer;
 
@@ -36,15 +40,18 @@ public RobotContainer() {
             case REAL -> {
                 rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo());
                 tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo());
+                intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo());
             }
             case REPLAY -> {
                 rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo());
                 tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo());
+                intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo());
             }
             case SIM -> {
                 robotVisualizer = new RobotVisualizer();
                 rollerSubsystem = new RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer));
                 tiltSubsystem = new TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer));
+                intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer));
             }
             default -> {
                 throw new RuntimeException("Did not specify Robot Mode");
@@ -93,6 +100,14 @@ public void putShuffleboardCommands() {
             SmartDashboard.putData(
                     "Tilt Down",
                     new TiltDown(tiltSubsystem));
+
+            SmartDashboard.putData(
+                    "Spin Intake",
+                    new SpinIntake(intakeSubsystem));
+
+            SmartDashboard.putData(
+                    "Flush Intake",
+                    new FlushIntake(intakeSubsystem));
         }
     }
 

diff --git a/src/main/java/frc/robot/commands/intake/FlushIntake.java b/src/main/java/frc/robot/commands/intake/FlushIntake.java
@@ -0,0 +1,43 @@
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+// Spins Intake
+public class FlushIntake extends LoggableCommand {
+  private final IntakeSubsystem subsystem;
+  private final Timer timer;
+
+  public FlushIntake(IntakeSubsystem subsystem) {
+    timer = new Timer();
+    this.subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+  }
+
+  @Override
+  public void execute() {
+    subsystem.setSpeed(-(Constants.INTAKE_SPEED));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    subsystem.stopMotors();
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (timer.hasElapsed(Constants.INTAKE_TIMEOUT - 3)) {
+      return true;
+    }
+    else {
+      return false;
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/commands/intake/SpinIntake.java b/src/main/java/frc/robot/commands/intake/SpinIntake.java
@@ -0,0 +1,43 @@
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+// Spins Intake
+public class SpinIntake extends LoggableCommand {
+  private final IntakeSubsystem subsystem;
+  private final Timer timer;
+
+  public SpinIntake(IntakeSubsystem subsystem) {
+    timer = new Timer();
+    this.subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+  }
+
+  @Override
+  public void execute() {
+    subsystem.setSpeed(Constants.INTAKE_SPEED);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    subsystem.stopMotors();
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (timer.hasElapsed(Constants.INTAKE_TIMEOUT)) {
+      return true;
+    }
+    else {
+      return false;
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/commands/roller/SpinRoller.java b/src/main/java/frc/robot/commands/roller/SpinRoller.java
@@ -5,6 +5,7 @@
 import frc.robot.subsystems.RollerSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
+// Spins roller on Algae Bye Bye
 public class SpinRoller extends LoggableCommand {
   private final RollerSubsystem subsystem;
   private final Timer timer;
@@ -34,7 +35,8 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     if (timer.hasElapsed(Constants.SPIN_TIMEOUT)) {
       return true;
-    } else{
+    }
+    else {
       return false;
     }
   }

diff --git a/src/main/java/frc/robot/commands/tilt/TiltDown.java b/src/main/java/frc/robot/commands/tilt/TiltDown.java
@@ -5,6 +5,7 @@
 import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
+// Tilts Algae Bye Bye arm down
 public class TiltDown extends LoggableCommand {
     private final TiltSubsystem subsystem;
     private final Timer timer;

diff --git a/src/main/java/frc/robot/commands/tilt/TiltUp.java b/src/main/java/frc/robot/commands/tilt/TiltUp.java
@@ -5,6 +5,7 @@
 import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
+// Tilts Algae Bye Bye arm up
 public class TiltUp extends LoggableCommand {
     private final TiltSubsystem subsystem;
     private final Timer timer;

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -0,0 +1,66 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkLowLevel;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.utils.logging.input.MotorLoggableInputs;
+import frc.robot.utils.logging.io.motor.MockSparkMaxIo;
+import frc.robot.utils.logging.io.motor.RealSparkMaxIo;
+import frc.robot.utils.logging.io.motor.SimSparkMaxIo;
+import frc.robot.utils.logging.io.motor.SparkMaxIo;
+import frc.robot.utils.simulation.MotorSimulator;
+import frc.robot.utils.simulation.RobotVisualizer;
+
+public class IntakeSubsystem extends SubsystemBase {
+public static final String LOGGING_NAME = "IntakeSubsystem";
+    private final SparkMaxIo io;
+
+    public IntakeSubsystem(SparkMaxIo io) {
+        this.io = io;
+    }
+
+    public void setSpeed(double speed) {
+        io.set(speed);
+    }
+
+    public void stopMotors() {
+        io.stopMotor();
+    }
+
+    @Override
+    public void periodic() {
+        io.periodic();
+    }
+
+    public static SparkMaxIo createMockIo() {
+        return new MockSparkMaxIo();
+    }
+
+    public static SparkMaxIo createRealIo() {
+        return new RealSparkMaxIo(LOGGING_NAME, createMotor(), MotorLoggableInputs.allMetrics());
+    }
+
+    public static SparkMaxIo createSimIo(RobotVisualizer visualizer) {
+        SparkMax motor = createMotor();
+        return new SimSparkMaxIo(LOGGING_NAME, motor, MotorLoggableInputs.allMetrics(),
+                new MotorSimulator(motor, visualizer.getIntakeLigament()));
+    }
+
+    private static SparkMax createMotor() {
+        SparkMax motor = new SparkMax(Constants.INTAKE_MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
+        SparkMaxConfig motorConfig = new SparkMaxConfig();
+        motorConfig.idleMode(SparkBaseConfig.IdleMode.kBrake);
+        motorConfig.smartCurrentLimit(Constants.NEO_CURRENT_LIMIT);
+        motor.configure(
+                motorConfig,
+                SparkBase.ResetMode.kResetSafeParameters,
+                SparkBase.PersistMode.kPersistParameters);
+
+        return motor;
+    }
+}