Setup intake interface

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

@@ -4,17 +4,22 @@
 
 package frc.robot;
 
+// import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.IntakeBaseCommand;
 import frc.robot.commands.ShooterEnable;
+import frc.robot.subsystems.intake.IntakeBase;
+import frc.robot.subsystems.intake.IntakeIOSim;
 import frc.robot.subsystems.shooter.ShooterIOSim;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 
 public class RobotContainer {
   public final CommandXboxController controller;
   public final ShooterSubsystem shooter;
+  public final IntakeBase intake;
 
   public RobotContainer() {
     controller = new CommandXboxController(0);
@@ -24,10 +29,13 @@ public RobotContainer() {
 
     if (swerveBot) {
       shooter = null;
+      intake = null;
     } else if (tankBot) {
       shooter = null;
+      intake = null;
     } else {
       shooter = new ShooterSubsystem(new ShooterIOSim());
+      intake = new IntakeBase(new IntakeIOSim());
     }
 
     configureBindings();
@@ -37,6 +45,11 @@ private void configureBindings() {
     shooter.setDefaultCommand(new InstantCommand(() -> shooter.disable(), shooter));
 
     controller.rightTrigger().whileTrue(new ShooterEnable(shooter));
+    intake.setDefaultCommand(
+        new IntakeBaseCommand(
+            intake,
+            () -> controller.rightBumper().getAsBoolean(),
+            () -> controller.leftBumper().getAsBoolean()));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/IntakeBaseCommand.java

@@ -0,0 +1,67 @@
+package frc.robot.commands;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeBase;
+import java.util.Map;
+import java.util.function.BooleanSupplier;
+
+public class IntakeBaseCommand extends Command {
+  IntakeBase intake;
+  BooleanSupplier inEnable;
+  GenericEntry voltsEntry;
+  ShuffleboardTab tab;
+  BooleanSupplier outEnable;
+
+  public IntakeBaseCommand(IntakeBase intake, BooleanSupplier inEnable, BooleanSupplier outEnable) {
+    this.intake = intake;
+    this.inEnable = inEnable;
+    this.outEnable = outEnable;
+
+    addRequirements(intake);
+    tab = Shuffleboard.getTab("Intake");
+    // Create volt entry under Shooter tab as a number sider with min = -1 and max = 1
+    voltsEntry =
+        tab.add("Volts", 0)
+            .withWidget(BuiltInWidgets.kNumberSlider)
+            .withProperties(Map.of("min", 0, "max", 12))
+            .getEntry();
+    voltsEntry.setValue(0.0);
+  }
+
+  @Override
+  public void initialize() {
+    intake.disable();
+  }
+
+  @Override
+  public void execute() {
+
+    // Turns off motors if No/All bumpers
+    if (inEnable.getAsBoolean() == true && outEnable.getAsBoolean() == true) {
+      // Disable the intake motors
+      intake.disable();
+      // System.out.println(outEnable.getAsBoolean());
+    } else if (inEnable.getAsBoolean() == false && outEnable.getAsBoolean() == false) {
+      intake.disable();
+      // System.out.println(outEnable.getAsBoolean());
+    } else if (inEnable.getAsBoolean() == true) { // Motors on (IN) if right bumper pressed
+      intake.setVoltage(voltsEntry.getDouble(0.0));
+      System.out.println(outEnable.getAsBoolean());
+
+      // Enable motors, It has to be called regularly for voltage compensation to work properly
+      intake.enable();
+    } else if (outEnable.getAsBoolean() == true) { // Motors on (Out) if right bumper pressed
+      intake.setVoltage(voltsEntry.getDouble(0.0) * -1);
+      System.out.println(outEnable.getAsBoolean());
+      // Enable motors, It has to be called regularly for voltage compensation to work properly
+      intake.enable();
+    } else { // Disable the intake motors
+      intake.disable();
+    }
+    // System.out.println(outEnable.getAsBoolean());
+  }
+}

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

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake;
+
+public interface Intake {
+  /** Disable the speed of the motor for the intake. */
+  public default void disable() {}
+
+  /** Enable the speed of the motor for the intake. */
+  public default void enable() {}
+
+  public default void setVoltage(double volts) {}
+}

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

@@ -0,0 +1,42 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class IntakeBase extends SubsystemBase implements Intake {
+  private final IntakeIO IO;
+  private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+  private double voltage;
+
+  public IntakeBase(IntakeIO IO) {
+
+    this.IO = IO;
+
+    voltage = 0;
+  }
+
+  @Override
+  // disable the intake
+  public void disable() {
+    voltage = 0;
+    IO.setVoltage(voltage);
+  }
+
+  @Override
+  // Enabling the intake
+  public void enable() {
+    // voltage = 1;
+    IO.setVoltage(voltage);
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    voltage = volts;
+  }
+
+  @Override
+  public void periodic() {
+    IO.updateInputs(inputs);
+    Logger.processInputs("Intake", inputs);
+  }
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeIOInputs {
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(IntakeIOInputs inputs) {}
+
+  /** Run open loop at the specified voltage. */
+  public default void setVoltage(double volts) {}
+}

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

@@ -0,0 +1,28 @@
+package frc.robot.subsystems.intake;
+
+// import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+
+public class IntakeIOSim implements IntakeIO {
+  private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004);
+  private double appliedVolts = 0.0;
+
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+
+    sim.update(0.02);
+
+    // inputs.positionRad = 0.0;
+    inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec();
+    inputs.appliedVolts = appliedVolts;
+    // inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()};
+
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    appliedVolts = volts;
+    sim.setInputVoltage(volts);
+  }
+}