Intake

.vscode/settings.json

@@ -29,8 +29,8 @@
     null
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
-  "spotlessGradle.format.enable": true,
-  "spotlessGradle.diagnostics.enable": false,
+  // "spotlessGradle.format.enable": true,
+  // "spotlessGradle.diagnostics.enable": false,
   "java.import.gradle.annotationProcessing.enabled": false,
   "java.completion.favoriteStaticMembers": [
     "org.junit.Assert.*",

build.gradle

@@ -187,43 +187,43 @@ task(eventDeploy) {
 createVersionFile.dependsOn(eventDeploy)
 
 // Spotless formatting
-project.compileJava.dependsOn(spotlessApply)
-spotless {
-    java {
-        target fileTree(".") {
-            include "**/*.java"
-            exclude "**/build/**", "**/build-*/**"
-        }
-        toggleOffOn()
-        googleJavaFormat()
-        removeUnusedImports()
-        trimTrailingWhitespace()
-        endWithNewline()
-    }
-    groovyGradle {
-        target fileTree(".") {
-            include "**/*.gradle"
-            exclude "**/build/**", "**/build-*/**"
-        }
-        greclipse()
-        indentWithSpaces(4)
-        trimTrailingWhitespace()
-        endWithNewline()
-    }
-    json {
-        target fileTree(".") {
-            include "**/*.json"
-            exclude "**/build/**", "**/build-*/**", ".wpilib/**"
-        }
-        gson().indentWithSpaces(2)
-    }
-    format "misc", {
-        target fileTree(".") {
-            include "**/*.md", "**/.gitignore"
-            exclude "**/build/**", "**/build-*/**"
-        }
-        trimTrailingWhitespace()
-        indentWithSpaces(2)
-        endWithNewline()
-    }
-}
+// project.compileJava.dependsOn(spotlessApply)
+// spotless {
+//     java {
+//         target fileTree(".") {
+//             include "**/*.java"
+//             exclude "**/build/**", "**/build-*/**"
+//         }
+//         toggleOffOn()
+//         googleJavaFormat()
+//         removeUnusedImports()
+//         trimTrailingWhitespace()
+//         endWithNewline()
+//     }
+//     groovyGradle {
+//         target fileTree(".") {
+//             include "**/*.gradle"
+//             exclude "**/build/**", "**/build-*/**"
+//         }
+//         greclipse()
+//         indentWithSpaces(4)
+//         trimTrailingWhitespace()
+//         endWithNewline()
+//     }
+//     json {
+//         target fileTree(".") {
+//             include "**/*.json"
+//             exclude "**/build/**", "**/build-*/**", ".wpilib/**"
+//         }
+//         gson().indentWithSpaces(2)
+//     }
+//     format "misc", {
+//         target fileTree(".") {
+//             include "**/*.md", "**/.gitignore"
+//             exclude "**/build/**", "**/build-*/**"
+//         }
+//         trimTrailingWhitespace()
+//         indentWithSpaces(2)
+//         endWithNewline()
+//     }
+// }

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

@@ -7,7 +7,10 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.revrobotics.spark.FeedbackSensor;
+
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -22,7 +25,9 @@
 public final class Constants {
   public static final Mode simMode = Mode.SIM;
   public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
-  public static final String canbus = "Omnivore";
+
+  // CAN bus that the devices are located on;
+  public static final CANBus CANBUS = new CANBus("canivore", "./logs/example.hoot");
 
   public static enum Mode {
     /** Running on a real robot. */
@@ -121,7 +126,7 @@ public enum IndexerModes {
     }
   }
 
-  public static final class IntakeConstants {
+  public static final class IntakeRollerIOConstants {
     public static final boolean attached = true;
     public static final boolean useRpm = false;
 
@@ -141,8 +146,10 @@ public static final class IntakeConstants {
     public static final boolean gravityType = false;
     public static final boolean breakType = false;
 
-    public static final FeedbackSensorSourceValue feedbackSensor =
+    public static final FeedbackSensorSourceValue feedbackSensorCTRE =
         FeedbackSensorSourceValue.FusedCANcoder;
+    public static final FeedbackSensor feedbackSensorREV = 
+        FeedbackSensor.kPrimaryEncoder;
 
     public static final double maxForwardOutput = 0.5;
     public static final double maxReverseOutput = -0.5;
@@ -157,21 +164,22 @@ public static final class IntakeConstants {
     public static final Current stallLimit = Units.Amps.of(80);
     public static final Current supplyLimit = Units.Amps.of(60);
 
-    public static final AngularVelocity intakeSpeed = Units.RPM.of(3000);
+    public static final AngularVelocity rollerIOSpeed = Units.RPM.of(3000);
     public static final AngularVelocity outtakeSpeed = Units.RPM.of(0);
     public static final AngularVelocity idleSpeed = Units.RPM.of(0);
 
-    public enum IntakeModes {
+    public enum RollerIOModes {
       IDLE(idleSpeed, 0.0),
-      INTAKE(intakeSpeed, 0.7),
-      OUTTAKE(outtakeSpeed, -0.4);
+      INTAKE(rollerIOSpeed, 8.4),
+      OUTTAKE(outtakeSpeed, -4.8);
 
       public AngularVelocity speed;
-      public double output;
+      // TODO: Make sure voltage is just output * 12 (for 12V)
+      public double voltage;
 
-      IntakeModes(AngularVelocity speed, double output) {
+      RollerIOModes(AngularVelocity speed, double voltage) {
         this.speed = speed;
-        this.output = output;
+        this.voltage = voltage;
       }
     }
   }

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

@@ -16,8 +16,8 @@
 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.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants.IntakeRollerIOConstants.RollerIOModes;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.drive.Drive;
@@ -26,10 +26,16 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
+import frc.robot.subsystems.intake.roller.Roller;
+import frc.robot.subsystems.intake.roller.RollerIO;
+import frc.robot.subsystems.intake.roller.RollerIOSim;
+import frc.robot.subsystems.intake.roller.RollerIOSparkFlex;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOLimelight;
 import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
+import frc.team5431.titan.core.joysticks.CommandXboxController;
+
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -42,9 +48,11 @@ public class RobotContainer {
   // Subsystems
   private final Drive drive;
   private final Vision vision;
+  private final Roller roller;
 
   // 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;
@@ -70,6 +78,10 @@ public RobotContainer() {
                 drive::addVisionMeasurement,
                 new VisionIOLimelight(camera0Name, drive::getRotation),
                 new VisionIOLimelight(camera1Name, drive::getRotation));
+        roller =
+            new Roller(
+                new RollerIOSparkFlex());
+
         // vision =
         // new Vision(
         // demoDrive::addVisionMeasurement,
@@ -109,6 +121,10 @@ public RobotContainer() {
                 drive::addVisionMeasurement,
                 new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose),
                 new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose));
+
+        roller = 
+            new Roller(
+                new RollerIOSim() {});
         break;
 
       default:
@@ -122,7 +138,10 @@ public RobotContainer() {
                 new ModuleIO() {});
 
         vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
+        roller = new Roller(new RollerIO() {});
+
         break;
+
     }
 
     // Set up auto routines
@@ -144,8 +163,8 @@ public RobotContainer() {
     autoChooser.addOption(
         "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
-    // Configure the button bindings
-    configureButtonBindings();
+    configureDriverBindings();
+    configureOperatorBindings();
   }
 
   /**
@@ -154,30 +173,30 @@ public RobotContainer() {
    * 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 configureDriverBindings() {
     // Default command, normal field-relative drive
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(
             drive,
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> -controller.getRightX()));
+            () -> -driver.getLeftY(),
+            () -> -driver.getLeftX(),
+            () -> -driver.getRightX()));
 
     // Lock to 0° when A button is held
-    controller
+    driver
         .a()
         .whileTrue(
             DriveCommands.joystickDriveAtAngle(
                 drive,
-                () -> -controller.getLeftY(),
-                () -> -controller.getLeftX(),
+                () -> -driver.getLeftY(),
+                () -> -driver.getLeftX(),
                 () -> Rotation2d.kZero));
 
     // Switch to X pattern when X button is pressed
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+    driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
-    // Reset gyro to 0° when B button is pressed
-    controller
+    // Reset gyro to 0° when B button is pressed
+    driver
         .b()
         .onTrue(
             Commands.runOnce(
@@ -187,23 +206,26 @@ private void configureButtonBindings() {
                     drive)
                 .ignoringDisable(true));
 
-    //     // Auto aim command example; code from AKit template.
-    //     @SuppressWarnings("resource")
-    //     PIDController aimController = new PIDController(0.2, 0.0, 0.0);
-    //     aimController.enableContinuousInput(-Math.PI, Math.PI);
-    //     controller
-    //         .button(1)
-    //         .whileTrue(
-    //             Commands.startRun(
-    //                 () -> {
-    //                   aimController.reset();
-    //                 },
-    //                 () -> {
-    //                   drive.run(0.0, aimController.calculate(vision.getTargetX(0).getRadians()));
-    //                 },
-    //                 drive));
+    // // Auto aim command example; code from AKit template.
+    // @SuppressWarnings("resource")
+    // PIDController aimController = new PIDController(0.2, 0.0, 0.0);
+    // aimController.enableContinuousInput(-Math.PI, Math.PI);
+    // controller
+    // .button(1)
+    // .whileTrue(
+    // Commands.startRun(
+    // () -> {
+    // aimController.reset();
+    // },
+    // () -> {
+    // drive.run(0.0, aimController.calculate(vision.getTargetX(0).getRadians()));
+    // },
+    // drive));
   }
 
+  private void configureOperatorBindings() {
+    operator.a().onTrue(roller.runIntakeCommand(RollerIOModes.INTAKE, true));
+  }
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

src/main/java/frc/robot/generated/TunerConstants.java

@@ -9,7 +9,6 @@
 
 import static edu.wpi.first.units.Units.*;
 
-import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.*;
 import com.ctre.phoenix6.hardware.*;
 import com.ctre.phoenix6.signals.*;
@@ -19,6 +18,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.*;
+import frc.robot.Constants;
 
 // Generated by the Tuner X Swerve Project Generator
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
@@ -84,10 +84,6 @@ public class TunerConstants {
   // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
   private static final Pigeon2Configuration pigeonConfigs = null;
 
-  // CAN bus that the devices are located on;
-  // All swerve devices must share the same CAN bus
-  public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
-
   // Theoretical free speed (m/s) at 12 V applied output;
   // This needs to be tuned to your individual robot
   public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
@@ -114,7 +110,7 @@ public class TunerConstants {
 
   public static final SwerveDrivetrainConstants DrivetrainConstants =
       new SwerveDrivetrainConstants()
-          .withCANBusName(kCANBus.getName())
+          .withCANBusName(Constants.CANBUS.getName())
           .withPigeon2Id(kPigeonId)
           .withPigeon2Configs(pigeonConfigs);
 

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

@@ -50,7 +50,7 @@
 
 public class Drive extends SubsystemBase {
   // TunerConstants doesn't include these constants, so they are declared locally
-  static final double ODOMETRY_FREQUENCY = TunerConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0;
+  static final double ODOMETRY_FREQUENCY = Constants.CANBUS.isNetworkFD() ? 250.0 : 100.0;
   public static final double DRIVE_BASE_RADIUS =
       Math.max(
           Math.max(

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

@@ -16,13 +16,14 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.Constants;
 import frc.robot.generated.TunerConstants;
 import java.util.Queue;
 
 /** IO implementation for Pigeon 2. */
 public class GyroIOPigeon2 implements GyroIO {
   private final Pigeon2 pigeon =
-      new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, TunerConstants.kCANBus);
+      new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, Constants.CANBUS);
   private final StatusSignal<Angle> yaw = pigeon.getYaw();
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;