feat: Add intake subsystem and commands

src/main/java/frc/robot/Constants.java

@@ -64,4 +64,10 @@ public enum Mode {
     public static final double DRIVE_BASE_LENGTH = 0.635;
     public static final double INITIAL_ROBOT_HEIGHT = 0;
 
+    public static final int INTAKE_MOTOR_ID = 23;
+    public static final int INTAKE_MOTOR_2_ID = 33;
+    public static final double INTAKE_SPEED = 0.3;
+    public static final double FLUSH_SPEED = -0.1;
+    public static final double INTAKE_TIMEOUT = 5;
+    public static final double FLUSH_TIMEOUT = 2;
 }

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.Flush;
+import frc.robot.commands.intake.Intake;
 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,28 @@ public RobotContainer() {
             case REAL -> {
                 rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo());
                 tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo());
+                intakeSubsystem = new IntakeSubsystem(
+                        IntakeSubsystem.createRealIo(Constants.INTAKE_MOTOR_ID),
+                        IntakeSubsystem.createRealIo(Constants.INTAKE_MOTOR_2_ID));
             }
             case REPLAY -> {
                 rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo());
                 tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo());
+                intakeSubsystem = new IntakeSubsystem(
+                        IntakeSubsystem.createMockIo(),
+                        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,
+                                IntakeSubsystem.createMotor(Constants.INTAKE_MOTOR_ID),
+                                robotVisualizer.getIntakeLigament()),
+                        IntakeSubsystem.createSimIo(robotVisualizer,
+                                IntakeSubsystem.createMotor(Constants.INTAKE_MOTOR_2_ID),
+                                robotVisualizer.getIntakeLigament2()));
             }
             default -> {
                 throw new RuntimeException("Did not specify Robot Mode");
@@ -93,6 +110,14 @@ public void putShuffleboardCommands() {
             SmartDashboard.putData(
                     "Tilt Down",
                     new TiltDown(tiltSubsystem));
+
+            SmartDashboard.putData(
+                    "Intake",
+                    new Intake(intakeSubsystem));
+
+            SmartDashboard.putData(
+                    "Flush",
+                    new Flush(intakeSubsystem));
         }
     }
 

src/main/java/frc/robot/commands/intake/Flush.java

@@ -0,0 +1,37 @@
+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;
+
+public class Flush extends LoggableCommand {
+  private final IntakeSubsystem subsystem;
+  private final Timer timer;
+
+  public Flush(IntakeSubsystem subsystem) {
+    timer = new Timer();
+    this.subsystem = subsystem;
+    addRequirements(subsystem);
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+  }
+
+  @Override
+  public void execute() {
+    subsystem.setSpeed(Constants.FLUSH_SPEED);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    subsystem.stopMotors();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return timer.hasElapsed(Constants.FLUSH_TIMEOUT);
+  }
+}

src/main/java/frc/robot/commands/intake/Intake.java

@@ -0,0 +1,37 @@
+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;
+
+public class Intake extends LoggableCommand {
+  private final IntakeSubsystem subsystem;
+  private final Timer timer;
+
+  public Intake(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() {
+    return timer.hasElapsed(Constants.INTAKE_TIMEOUT);
+  }
+}

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -0,0 +1,76 @@
+// 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 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;
+import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d;
+
+// The Intake subsystem spins the wheel that intakes the algae.
+
+public class IntakeSubsystem extends SubsystemBase {
+    public static final String LOGGING_NAME = "IntakeSubsystem";
+    private final SparkMaxIo io;
+    private final SparkMaxIo io2;
+
+    public IntakeSubsystem(SparkMaxIo io, SparkMaxIo io2) {
+        this.io = io;
+        this.io2 = io2;
+    }
+
+    public void setSpeed(double speed) {
+        io.set(speed);
+        io2.set(-speed);
+    }
+
+    public void stopMotors() {
+        io.stopMotor();
+        io2.stopMotor();
+    }
+
+    @Override
+    public void periodic() {
+        io.periodic();
+        io2.periodic();
+    }
+
+    public static SparkMaxIo createMockIo() {
+        return new MockSparkMaxIo();
+    }
+
+    public static SparkMaxIo createRealIo(int motorId) {
+        return new RealSparkMaxIo(LOGGING_NAME, createMotor(motorId), MotorLoggableInputs.allMetrics());
+    }
+
+    public static SparkMaxIo createSimIo(RobotVisualizer visualizer, SparkMax motor, LoggedMechanismLigament2d ligament) {
+        return new SimSparkMaxIo(LOGGING_NAME, motor, MotorLoggableInputs.allMetrics(),
+                new MotorSimulator(motor, ligament));
+    }
+
+    public static SparkMax createMotor(int port) {
+        SparkMax motor = new SparkMax(port, 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;
+    }
+}

src/main/java/frc/robot/utils/simulation/RobotVisualizer.java

@@ -13,6 +13,8 @@ public class RobotVisualizer {
     private final LoggedMechanism2d mech2d = new LoggedMechanism2d(2, Units.feetToMeters(7));
     private final LoggedMechanismLigament2d tiltLigament;
     private final LoggedMechanismLigament2d rollerLigament;
+    private final LoggedMechanismLigament2d intakeLigament;
+    private final LoggedMechanismLigament2d intakeLigament2;
 
     public RobotVisualizer() {
         LoggedMechanismRoot2d root =
@@ -34,6 +36,27 @@ public RobotVisualizer() {
                 this.tiltLigament.append(
                         new LoggedMechanismLigament2d(
                                 "Roller", 0.05, 180, 5, new Color8Bit(Color.kGreen)));
+
+        LoggedMechanismLigament2d intakeRiserLigament =
+                root.append(
+                        new LoggedMechanismLigament2d(
+                                "IntakeRiser", 0.7, 30, 5, new Color8Bit(Color.kRed)));
+        this.intakeLigament =
+                intakeRiserLigament.append(
+                        new LoggedMechanismLigament2d(
+                                "Intake", 0.05, 180, 5, new Color8Bit(Color.kOrange)));
+        this.intakeLigament2 =
+                intakeRiserLigament.append(
+                        new LoggedMechanismLigament2d(
+                                "Intake2", 0.05, 180, 5, new Color8Bit(Color.kPurple)));
+    }
+
+    public LoggedMechanismLigament2d getIntakeLigament() {
+        return intakeLigament;
+    }
+
+    public LoggedMechanismLigament2d getIntakeLigament2() {
+        return intakeLigament2;
     }
 
     public LoggedMechanismLigament2d getRollerLigament() {