Configuration

src/main/java/frc4388/robot/RobotContainer.java

@@ -11,6 +11,7 @@ import java.io.File;
 
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -19,30 +20,23 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // Commands
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc4388.robot.commands.MoveForTimeCommand;
-import frc4388.robot.commands.MoveUntilSuply;
 import frc4388.robot.constants.Constants;
-import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.constants.Constants.OIConstants;
 import frc4388.robot.constants.Constants.SimConstants.Mode;
 import frc4388.robot.constants.FieldConstants;
 // Subsystems
 import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.shooter.Shooter;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.compute.TimesNegativeOne;
-import frc4388.utility.configurable.ConfigurableString;
 import frc4388.utility.controller.DeadbandedXboxController;
 // Autos
 import frc4388.utility.controller.VirtualController;
@@ -65,7 +59,7 @@ public class RobotContainer {
     public final LED m_robotLED = new LED();
     //Testing of Colors
     
-    public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
+    public final Vision m_vision = new Vision();
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     private Boolean operatorManualMode = false;
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -116,19 +110,10 @@ public class RobotContainer {
         DriverStation.silenceJoystickConnectionWarning(true);
 
         m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-
+            // Drive normally
-            // IF the driver is holding the aim button, aim the robot towards the hub
+            m_robotSwerveDrive.driveWithInput(
-            if(m_driverXbox.getRightTriggerAxis() > 0.5) {
+                getDeadbandedDriverController().getLeft(),
-                // Aim 
+                getDeadbandedDriverController().getRight(),true);
-                m_robotSwerveDrive.driveFacingPosition(
-                    getDeadbandedDriverController().getLeft(),
-                    FieldConstants.BLUE_HUB_POS);
-            } else {
-                // Drive normally
-                m_robotSwerveDrive.driveWithInput(
-                    getDeadbandedDriverController().getLeft(),
-                    getDeadbandedDriverController().getRight(),true);
-            }
 
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
@@ -170,12 +155,46 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
 
+       
+
         new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
 
+        // IF the driver is holding the aim button, aim the robot towards the hub
+        new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
+            .whileTrue(new RunCommand(
+                () -> {
+                m_robotSwerveDrive.driveFacingPosition(
+                    getDeadbandedDriverController().getLeft(),
+                    FieldConstants.BLUE_HUB_POS);
+                }, m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+
+
+        // IF the driver is holding the aim button, aim the robot towards the hub
+        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+            .whileTrue(new RunCommand(
+                () -> {
+                    m_robotSwerveDrive.driveIntake(
+                        getDeadbandedDriverController().getLeft()
+                    );
+                }, m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+
+        // D-PAD fine alignment
+        new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
+            .whileTrue(new RunCommand(
+                () -> m_robotSwerveDrive.driveFine(
+                    new Translation2d(
+                        1, 
+                        Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
+                    ), 
+                    getDeadbandedDriverController().getRight(), 0.15
+                ), m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
    }
 
 //.onTrue(new InstantCommand(()  -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));

src/main/java/frc4388/robot/RobotMap.java

@@ -46,8 +46,8 @@ public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    public final VisionIO leftCamera;
+    // public final VisionIO leftCamera;
-    public final VisionIO rightCamera;
+    // public final VisionIO rightCamera;
 
     // public final LiDAR lidar = new 
 
@@ -62,21 +62,21 @@ public class RobotMap {
     public final SwerveIO swerveDrivetrain;
 
     // /* Elevator Subsystem */
-    public final ShooterIO shooterIO;
+    // public final ShooterIO shooterIO;
-    public final IntakeIO intakeIO;
+    // public final IntakeIO intakeIO;
 
     public RobotMap(SimConstants.Mode mode) {
         switch (mode) {
             case REAL:
                 // Configure cameras
-                PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+                // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
-                PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+                // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
 
-                leftCamera =  new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS);                ;
+                // leftCamera =  new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS);                ;
-                rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS);
+                // rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS);
 
-                FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
+                // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
-                FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
+                // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
 
                 // // Configure LiDAR
                 // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
@@ -98,16 +98,16 @@ public class RobotMap {
                 TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id);
                 
                 //Configure Intake
-                TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id);
+                // TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id);
-                TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id);
+                // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id);
 
                 // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
                 // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
                 // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-                shooterIO = new ShooterReal(shooter1, shooter2, indexer);
+                // shooterIO = new ShooterReal(shooter1, shooter2, indexer);
 
-                intakeIO = new IntakeReal(arm, roller);
+                // intakeIO = new IntakeReal(arm, roller);
 
                 // Fault
                 FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
@@ -116,8 +116,8 @@ public class RobotMap {
                 FaultTalonFX.addDevice(shooter1, "Shooter1");
                 FaultTalonFX.addDevice(shooter2, "Shooter2");
                 FaultTalonFX.addDevice(indexer, "Indexer");
-                FaultTalonFX.addDevice(arm, "Arm");
+                // FaultTalonFX.addDevice(arm, "Arm");
-                FaultTalonFX.addDevice(roller, "Roller");
+                // FaultTalonFX.addDevice(roller, "Roller");
                 
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
@@ -136,11 +136,11 @@ public class RobotMap {
             // case SIM:
             //     break;
             default:
-                leftCamera = new VisionIO() {};
+                // leftCamera = new VisionIO() {};
-                rightCamera = new VisionIO() {};
+                // rightCamera = new VisionIO() {};
                 swerveDrivetrain = new SwerveIO() {};
-                intakeIO = new IntakeIO() {};
+                // intakeIO = new IntakeIO() {};
-                shooterIO = new ShooterIO() {};
+                // shooterIO = new ShooterIO() {};
                 break;
         }
     }

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 28;
+  public static final int GIT_REVISION = 29;
-  public static final String GIT_SHA = "5f4d7887ffc93ef535cef23511da086cd7f6d8bd";
+  public static final String GIT_SHA = "dc33af165d04720f576a5d490987d6ee079c4b8d";
-  public static final String GIT_DATE = "2026-01-31 15:33:14 MST";
+  public static final String GIT_DATE = "2026-01-31 21:38:10 MST";
   public static final String GIT_BRANCH = "Subsystem-Boilerplate";
-  public static final String BUILD_DATE = "2026-01-31 21:37:04 MST";
+  public static final String BUILD_DATE = "2026-02-02 19:39:42 MST";
-  public static final long BUILD_UNIX_TIME = 1769920624950L;
+  public static final long BUILD_UNIX_TIME = 1770086382841L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -287,6 +287,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     
+    
     public void driveIntake(Translation2d leftStick){
         // if (invert){
         //     Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY());

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -31,6 +31,7 @@ import frc4388.utility.structs.Gains;
 // No mans land
 // Beware, there be dragons.
 public final class SwerveDriveConstants {
+    public static final String CANBUS_NAME = "drivetrain";
     public static final double MAX_ROT_SPEED        = Math.PI * 2;
     public static final double AUTO_MAX_ROT_SPEED = 1.5;
     public static final double MIN_ROT_SPEED        = 1.0;
@@ -79,7 +80,7 @@ public final class SwerveDriveConstants {
 
     private static final class ModuleSpecificConstants { //2025
         //Front Left
-        private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
+        private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.35);
         private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
         private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
         private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -87,7 +88,7 @@ public final class SwerveDriveConstants {
         private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
         
         //Front Right
-        private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
+        private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3);
         private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
         private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
         private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -95,7 +96,7 @@ public final class SwerveDriveConstants {
         private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
 
         //Back Left
-        private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
+        private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438);
         private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
         private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
         private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -103,7 +104,7 @@ public final class SwerveDriveConstants {
         private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
         
         //Back Right
-        private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
+        private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05);
         private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
         private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
         private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
@@ -208,7 +209,7 @@ public final class SwerveDriveConstants {
     }
 
     public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
-        .withPigeon2Id(IDs.DRIVE_PIGEON.id);
+        .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME);
 
     private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
         new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.