Test offset

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

@@ -251,6 +251,15 @@ public class RobotContainer {
                     ShooterConstants.AIM_LEAD_TIME.get()
                     );
                 }, m_robotSwerveDrive)
+                // () -> {
+                // m_robotSwerveDrive.driveFacingVelocity(
+                //     getDeadbandedDriverController().getLeft(),
+                //     FieldPositions.HUB_POSITION,
+                //     ShooterConstants.AIM_LEAD_TIME.get(),
+                //     m_robotShooter.getBallVelocity(),
+                //     m_robotShooter.getDistanceToHub()
+                //     );
+                // }, m_robotSwerveDrive)
             );
 
         // D-PAD fine alignment

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 = 147;
+  public static final int GIT_REVISION = 149;
-  public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6";
+  public static final String GIT_SHA = "50b8312c48be14a559876064cdddbcd27bcb8ef7";
-  public static final String GIT_DATE = "2026-03-07 20:08:15 MST";
+  public static final String GIT_DATE = "2026-03-10 08:39:05 MDT";
-  public static final String GIT_BRANCH = "PikesPeak";
+  public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT";
+  public static final String BUILD_DATE = "2026-03-10 21:05:03 MDT";
-  public static final long BUILD_UNIX_TIME = 1773001639109L;
+  public static final long BUILD_UNIX_TIME = 1773198303540L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -31,6 +31,7 @@ public class Shooter extends SubsystemBase {
 
     // Supplier<Pose2d> m_swervePoseSupplier;
     public boolean badShooterVelocity;
+    public double distanceToHub = 5;
 
 
     public Shooter(
@@ -76,7 +77,9 @@ public class Shooter extends SubsystemBase {
         this.mode = ShooterMode.Idle;
     }
 
-
+    public double getDistanceToHub(){
+        return distanceToHub;
+    }
 
     public void allowShooting() {
         shooterButtonReady = true;
@@ -86,6 +89,9 @@ public class Shooter extends SubsystemBase {
         shooterButtonReady = false;
     }
 
+    public AngularVelocity getBallVelocity() {
+        return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond)));
+    }
 
     @AutoLogOutput
     public ShooterMode getMode() {
@@ -126,7 +132,7 @@ public class Shooter extends SubsystemBase {
         Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
 
         // Get the robot's aim distance to hub
-        double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
+        distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
 
         //Center of hub to cameras in inches
         Logger.recordOutput("Hub Dist", distanceToHub);

src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java

@@ -26,6 +26,7 @@ public interface ShooterIO {
 
         AngularVelocity motor1Velocity = RotationsPerSecond.of(0);
         AngularVelocity motor2Velocity = RotationsPerSecond.of(0);
+        AngularVelocity indexerVelocity = RotationsPerSecond.of(0);
         double indexerOutput = 0;
         
         Current motor1Current = Amps.of(0);

src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java

@@ -95,6 +95,7 @@ public class ShooterReal implements ShooterIO {
 
         state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO);
         state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO);
+        state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
         state.indexerOutput = m_indexerMotor.get();
         // state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
 

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

@@ -5,6 +5,7 @@
 package frc4388.robot.subsystems.swerve;
 
 import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import java.util.function.Supplier;
 
@@ -23,6 +24,7 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -377,6 +379,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         driveFieldAngle(leftStick, ang);
     }
 
+
+    public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) {
+
+        Translation2d robotSpeed = new Translation2d(
+            chassisSpeeds.vxMetersPerSecond, 
+            chassisSpeeds.vyMetersPerSecond
+        );
+
+        if (ballVelocity.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){
+            double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond));
+            fieldPos =  new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
+            Logger.recordOutput("Aim Offset", aimOffset);
+        }
+
+        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);        
+
+        Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
+
+
+        driveFieldAngle(leftStick, ang);
+    }
+
     public Pose2d getCurrentPose(){
         return state.currentPose;
     }