intake

.factorypath

@@ -0,0 +1,4 @@
+<factorypath>
+    <factorypathentry kind="EXTJAR" id="/Users/rohangodha/.gradle/caches/modules-2/files-2.1/org.littletonrobotics.akit.junction/junction-autolog/3.0.0/abaf89ad1ac7acc95dbc0d3f0785a39acf98dd0e/junction-autolog-3.0.0.jar" enabled="true" runInBatchMode="false"/>
+    <factorypathentry kind="EXTJAR" id="/Users/rohangodha/.gradle/caches/modules-2/files-2.1/com.squareup/javapoet/1.13.0/d6562d385049f35eb50403fa86bb11cce76b866a/javapoet-1.13.0.jar" enabled="true" runInBatchMode="false"/>
+</factorypath>

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

@@ -22,16 +22,30 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */
     REAL,
-
     /** Running a physics simulator. */
     SIM,
-
     /** Replaying from a log file. */
     REPLAY
   }
+
+  public static class DriveConstants {
+    // Front Left, Front Right, Back Left, Back Right
+    // IN DEGREES
+    public static final double[] moduleAngles = new double[] {0, 0, 0, 0};
+
+    public static final int[] turnCanIds = new int[] {1, 4, 7, 10};
+    public static final int[] driveCanIds = new int[] {0, 3, 6, 9};
+    public static final int[] canCoderIds = new int[] {2, 5, 8, 11};
+  }
+
+  public static class IntakeConstants {
+    public static final int intakeCANId = 12;
+
+    public static final double intakeOnPower = 0.1;
+  }
 }

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

@@ -29,61 +29,83 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIO;
+import frc.robot.subsystems.intake.IntakeIOReal;
+import frc.robot.subsystems.intake.IntakeIOSim;
+
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // Subsystems
   private final Drive drive;
+  private final Intake intake;
 
   // Controller
-  private final CommandXboxController controller = new CommandXboxController(0);
+  private final CommandXboxController driver = new CommandXboxController(0);
+  private final CommandXboxController operator = new CommandXboxController(1);
 
   // Dashboard inputs
   private final LoggedDashboardChooser<Command> autoChooser;
-  private final LoggedDashboardNumber flywheelSpeedInput =
-      new LoggedDashboardNumber("Flywheel Speed", 1500.0);
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  /**
+   * The container for the robot. Contains subsystems, OI devices, and commands.
+   */
   public RobotContainer() {
     switch (Constants.currentMode) {
       case REAL:
-        drive =
-            new Drive(
-                new GyroIOPigeon2(true),
-                new ModuleIOTalonFX(0),
-                new ModuleIOTalonFX(1),
-                new ModuleIOTalonFX(2),
-                new ModuleIOTalonFX(3));
+        drive = new Drive(
+            new GyroIOPigeon2(true),
+            new ModuleIOTalonFX(0),
+            new ModuleIOTalonFX(1),
+            new ModuleIOTalonFX(2),
+            new ModuleIOTalonFX(3));
+
+        intake = new Intake(new IntakeIOReal());
 
         break;
 
       case SIM:
         // Sim robot, instantiate physics sim IO implementations
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim());
+        drive = new Drive(
+            new GyroIO() {
+            },
+            new ModuleIOSim(),
+            new ModuleIOSim(),
+            new ModuleIOSim(),
+            new ModuleIOSim());
+
+        intake = new Intake(new IntakeIOSim());
+
         break;
 
       default:
         // Replayed robot, disable IO implementations
-        drive =
-            new Drive(
-                new GyroIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {},
-                new ModuleIO() {});
+        drive = new Drive(
+            new GyroIO() {
+            },
+            new ModuleIO() {
+            },
+            new ModuleIO() {
+            },
+            new ModuleIO() {
+            },
+            new ModuleIO() {
+            });
+
+        intake = new Intake(new IntakeIO() {
+        });
+
         break;
     }
 
@@ -101,28 +123,34 @@ public RobotContainer() {
   }
 
   /**
-   * Use this method to define your button->command mappings. Buttons can be created by
+   * Use this method to define your button->command mappings. Buttons can be
+   * created by
    * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@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() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
             drive,
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-    controller
+            () -> -driver.getLeftY(),
+            () -> -driver.getLeftX(),
+            () -> -driver.getRightX()));
+
+    driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+
+    driver
         .b()
         .onTrue(
             Commands.runOnce(
-                    () ->
-                        drive.setPose(
-                            new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
-                    drive)
+                () -> drive.setPose(
+                    new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+                drive)
                 .ignoringDisable(true));
+
+    operator.a().onTrue(intake.on());
+    operator.b().onTrue(intake.off());
   }
 
   /**

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -24,18 +24,18 @@
 
 /** IO implementation for Pigeon2 */
 public class GyroIOPigeon2 implements GyroIO {
-  private final Pigeon2 pigeon = new Pigeon2(20);
-  private final StatusSignal<Double> yaw = pigeon.getYaw();
+  private final Pigeon2 pigeo = new Pigeon2(20);
+  private final StatusSignal<Double> yaw = pigeo.getYaw();
   private final Queue<Double> yawPositionQueue;
-  private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZWorld();
+  private final StatusSignal<Double> yawVelocity = pigeo.getAngularVelocityZWorld();
 
   public GyroIOPigeon2(boolean phoenixDrive) {
-    pigeon.getConfigurator().apply(new Pigeon2Configuration());
-    pigeon.getConfigurator().setYaw(0.0);
+    pigeo.getConfigurator().apply(new Pigeon2Configuration());
+    pigeo.getConfigurator().setYaw(0.0);
     yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY);
     yawVelocity.setUpdateFrequency(100.0);
-    pigeon.optimizeBusUtilization();
-    yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw());
+    pigeo.optimizeBusUtilization();
+    yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeo, pigeo.getYaw());
   }
 
   @Override

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -25,6 +25,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants.DriveConstants;
 import java.util.Queue;
 
 /**
@@ -65,34 +66,13 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final Rotation2d absoluteEncoderOffset;
 
   public ModuleIOTalonFX(int index) {
-    switch (index) {
-      case 0:
-        driveTalon = new TalonFX(0);
-        turnTalon = new TalonFX(1);
-        cancoder = new CANcoder(2);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      case 1:
-        driveTalon = new TalonFX(3);
-        turnTalon = new TalonFX(4);
-        cancoder = new CANcoder(5);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      case 2:
-        driveTalon = new TalonFX(6);
-        turnTalon = new TalonFX(7);
-        cancoder = new CANcoder(8);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      case 3:
-        driveTalon = new TalonFX(9);
-        turnTalon = new TalonFX(10);
-        cancoder = new CANcoder(11);
-        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
-        break;
-      default:
-        throw new RuntimeException("Invalid module index");
+    if (index >= 4) {
+      throw new RuntimeException("Invalid module index");
     }
+    driveTalon = new TalonFX(DriveConstants.driveCanIds[index]);
+    turnTalon = new TalonFX(DriveConstants.turnCanIds[index]);
+    cancoder = new CANcoder(DriveConstants.canCoderIds[index]);
+    absoluteEncoderOffset = Rotation2d.fromDegrees(DriveConstants.moduleAngles[index]);
 
     var driveConfig = new TalonFXConfiguration();
     driveConfig.CurrentLimits.StatorCurrentLimit = 40.0;

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

@@ -0,0 +1,38 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.Logger;
+import frc.robot.Constants.IntakeConstants;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Intake extends SubsystemBase {
+  public final IntakeIO io;
+  public final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+  public Intake(IntakeIO io) {
+    this.io = io;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Intake", inputs);
+  }
+
+  public void turnOn() {
+    io.setPower(IntakeConstants.intakeOnPower);
+  }
+
+  public void turnOff() {
+    io.setPower(0.0);
+  }
+
+  public Command on() {
+    return new InstantCommand(this::turnOn, this);
+  }
+
+  public Command off() {
+    return new InstantCommand(this::turnOff, this);
+  }
+}

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

@@ -0,0 +1,16 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeIOInputs {
+    public double current = 0.0;
+  }
+
+  public default void updateInputs(IntakeIOInputs inputs) {}
+
+  public default void setPower(double power) {}
+
+  public default void setVoltage(double voltage) {}
+}

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

@@ -0,0 +1,35 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.Constants.IntakeConstants;
+
+public class IntakeIOReal implements IntakeIO {
+  private final TalonFX motor;
+  private final StatusSignal<Double> current;
+
+  public IntakeIOReal() {
+    motor = new TalonFX(IntakeConstants.intakeCANId);
+    current = motor.getStatorCurrent();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(20.0, current);
+    motor.optimizeBusUtilization();
+  }
+
+  @Override
+  public void updateInputs(IntakeIO.IntakeIOInputs inputs) {
+    BaseStatusSignal.refreshAll(current);
+    inputs.current = current.getValue();
+  }
+
+  @Override
+  public void setPower(double power) {
+    motor.set(power);
+  }
+
+  @Override
+  public void setVoltage(double voltage) {
+    motor.setVoltage(voltage);
+  }
+}

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

@@ -0,0 +1,5 @@
+package frc.robot.subsystems.intake;
+
+// TODO
+public class IntakeIOSim implements IntakeIO {
+}