New shooter model and auto tester

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 = 180;
-  public static final String GIT_SHA = "89a1f34a4a4751acadc1509890fed15d6d870930";
-  public static final String GIT_DATE = "2026-03-20 23:50:03 MDT";
+  public static final int GIT_REVISION = 181;
+  public static final String GIT_SHA = "3c1b94a2d86a624a9320493a0240ee552491beca";
+  public static final String GIT_DATE = "2026-03-21 13:04:40 MDT";
   public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-21 13:00:47 MDT";
-  public static final long BUILD_UNIX_TIME = 1774119647384L;
+  public static final String BUILD_DATE = "2026-03-21 13:30:10 MDT";
+  public static final long BUILD_UNIX_TIME = 1774121410165L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -39,7 +39,7 @@ public class    ShooterConstants {
     public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.);
 
     
-    public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1);
+    public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1);
 
     // Shoot mode tolerances
     public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);

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

@@ -141,6 +141,8 @@ public class ShooterReal implements ShooterIO {
         //     Math.abs(currentLimit.in(Amps)) > current &&
         //     Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
         // ) {
+            state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput);
+            state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput);
             m_shooter1Motor.set(percentOutput);
             m_shooter2Motor.set(percentOutput);
         // } else {

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

@@ -416,19 +416,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     // Drive with the robot facing towards a specific position
     public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
-        // Get the current speed of the robot
-        Translation2d robotSpeed = new Translation2d(
-            chassisSpeeds.vxMetersPerSecond, 
-            chassisSpeeds.vyMetersPerSecond
-        );
-
-        // Calculate a point to aim ahead of the actual position.
-        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
-
-        // Calculate the angle between the current position and the lead position
+        Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
+        double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY());
+        if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){
+        if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) {
+            if (TimesNegativeOne.isRed){
+                    robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond);
+            } else {
+                robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond);
+            }
+        } }
+        Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
         Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
-
-
+        Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d());
+        Logger.recordOutput("Lead Aim", fieldPosLeadLog);
         driveFieldAngle(leftStick, ang);
     }
 
@@ -541,13 +542,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) {
-        Translation2d robotSpeed = new Translation2d(
-            chassisSpeeds.vxMetersPerSecond, 
-            chassisSpeeds.vyMetersPerSecond
-        );
-        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
-        System.out.println("field pos lead: " + fieldPosLead);
-        Logger.recordOutput("Auto Aim", fieldPosLead);
+        Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
+
+        Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
         m_rotationOverrideTarget = fieldPosLead;
         m_useRotationOverride = true;
     }

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

@@ -73,8 +73,7 @@ public final class SwerveDriveConstants {
     public static final boolean INVERT_Y = true;
     public static final boolean INVERT_ROTATION = false;
 
-    public static ConfigurableDouble distanceTolerence = new ConfigurableDouble("Distance Tolerence", 0.5);
-
+    public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05);
     // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
 
     public static final class ModuleSpecificConstants { //2026