Intake Subsystem (#10)

Co-authored-by: Aidan Underwood <[email protected]>
Co-authored-by: Alex Resnick <[email protected]>

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

@@ -28,6 +28,14 @@ public static final class ElevatorWrist {
             public static final int TALON_ID = -1;
             public static final int NEO_ID = -1;
         }
+
+        /**
+         * Intake and indexer motor constants
+         */
+        public static final class Intake {
+            public static final int INTAKE_MOTOR_ID = -1;
+            public static final int INDEXER_MOTOR_ID = -1;
+        }
     }
 
     /**
@@ -64,6 +72,4 @@ public static final class ElevatorWristConstants {
      */
     public static final class Pneumatics {
     }
-
-
 }

src/main/java/frc/robot/RobotContainer.java

@@ -12,6 +12,9 @@
 import frc.robot.subsystems.drive.Drivetrain;
 import frc.robot.subsystems.drive.DrivetrainIO;
 import frc.robot.subsystems.drive.DrivetrainVictorSP;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIO;
+import frc.robot.subsystems.intake.IntakeIOFalcon;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -29,6 +32,7 @@ public class RobotContainer {
 
     /* Subsystems */
     private Drivetrain drivetrain;
+    private Intake intake;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -39,12 +43,14 @@ public RobotContainer(RobotRunType runtimeType) {
         switch (runtimeType) {
             case kReal:
                 drivetrain = new Drivetrain(new DrivetrainVictorSP());
+                intake = new Intake(new IntakeIOFalcon());
                 break;
             case kSimulation:
                 // drivetrain = new Drivetrain(new DrivetrainSim() {});
                 break;
             default:
                 drivetrain = new Drivetrain(new DrivetrainIO() {});
+                intake = new Intake(new IntakeIO() {});
         }
         // Configure the button bindings
         configureButtonBindings();
@@ -56,7 +62,9 @@ public RobotContainer(RobotRunType runtimeType) {
      * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
      * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
-    private void configureButtonBindings() {}
+    private void configureButtonBindings() {
+        operator.a().whileTrue(intake.runIntakeMotor());
+    }
 
     /**
      * Gets the user's selected autonomous command.

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

@@ -0,0 +1,54 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Intake Subsystem
+ */
+public class Intake extends SubsystemBase {
+    private IntakeIO io;
+    private IntakeInputsAutoLogged intakeAutoLogged = new IntakeInputsAutoLogged();
+
+    public Intake(IntakeIO io) {
+        this.io = io;
+        io.updateInputs(intakeAutoLogged);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(intakeAutoLogged);
+        Logger.processInputs("Intake", intakeAutoLogged);
+    }
+
+    public void setIntakeMotor(double percentage) {
+        Logger.recordOutput("/Intake/Intake Percentage", percentage);
+        io.setIntakeMotorPercentage(percentage);
+    }
+
+    public void setIndexerMotor(double percentage) {
+        Logger.recordOutput("/Intake/Indexer Percentage", percentage);
+        io.setIndexerMotorPercentage(percentage);
+    }
+
+    public boolean getSensorStatus() {
+        return intakeAutoLogged.sensorStatus;
+    }
+
+    /**
+     * Command to run the intake motor and indexer until the sensor trips
+     *
+     * @return {@link Command} to run the intake and indexer motors
+     */
+    public Command runIntakeMotor() {
+        return Commands.startEnd(() -> {
+            setIntakeMotor(0.5);
+            setIndexerMotor(0.5);
+        }, () -> {
+            setIntakeMotor(0);
+            setIndexerMotor(0);
+        }, this).until(() -> getSensorStatus()).unless(() -> getSensorStatus());
+    }
+}

src/main/java/frc/robot/subsystems/intake/IntakeIO.java

@@ -0,0 +1,31 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/**
+ * Intake IO Interface
+ */
+public interface IntakeIO {
+
+    /**
+     * Intake Inputs to Log
+     */
+    @AutoLog
+    public static class IntakeInputs {
+        public double intakeSupplyVoltage;
+        public double indexerSupplyVoltage;
+        public double intakeMotorVoltage;
+        public double indexerMotorVoltage;
+        public double intakeAmps;
+        public double indexerAmps;
+        public double intakeRPM;
+        public double indexerRPM;
+        public boolean sensorStatus;
+    }
+
+    public default void updateInputs(IntakeInputs inputs) {}
+
+    public default void setIntakeMotorPercentage(double percent) {}
+
+    public default void setIndexerMotorPercentage(double percent) {}
+}

src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java

@@ -0,0 +1,47 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.Constants;
+
+/**
+ * Intake IO Layer with real motors and sensors
+ */
+public class IntakeIOFalcon implements IntakeIO {
+
+    private final TalonFX intakeMotor =
+        new TalonFX(Constants.Motors.Intake.INTAKE_MOTOR_ID, "canivore");
+    private final TalonFX indexerMotor =
+        new TalonFX(Constants.Motors.Intake.INDEXER_MOTOR_ID, "canivore");
+
+    private final DutyCycleOut intakeDutyCycleOut = new DutyCycleOut(0);
+    private final DutyCycleOut indexerDutyCycleOut = new DutyCycleOut(0);
+
+    /**
+     * Intake IO Layer with real motors and sensors
+     */
+    public IntakeIOFalcon() {}
+
+    @Override
+    public void updateInputs(IntakeInputs inputs) {
+        inputs.intakeSupplyVoltage = intakeMotor.getSupplyVoltage().getValueAsDouble();
+        inputs.intakeMotorVoltage = intakeMotor.getMotorVoltage().getValueAsDouble();
+        inputs.intakeAmps = intakeMotor.getStatorCurrent().getValueAsDouble();
+        inputs.intakeRPM = intakeMotor.getVelocity().getValueAsDouble();
+        inputs.indexerSupplyVoltage = indexerMotor.getSupplyVoltage().getValueAsDouble();
+        inputs.indexerMotorVoltage = indexerMotor.getMotorVoltage().getValueAsDouble();
+        inputs.indexerAmps = indexerMotor.getStatorCurrent().getValueAsDouble();
+        inputs.indexerRPM = indexerMotor.getVelocity().getValueAsDouble();
+        inputs.sensorStatus = false;
+    }
+
+    @Override
+    public void setIntakeMotorPercentage(double percent) {
+        intakeMotor.setControl(intakeDutyCycleOut.withOutput(percent));
+    }
+
+    @Override
+    public void setIndexerMotorPercentage(double percent) {
+        indexerMotor.setControl(indexerDutyCycleOut.withOutput(percent));
+    }
+}