Drive shoot auto and shooter tolerence

src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto

@@ -0,0 +1,38 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot driving across"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Shoot Driving"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Shoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Shoot driving across.path

@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9932621113872893,
+        "y": 6.3390412980405895
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 1.6628461538461545
+      },
+      "prevControl": {
+        "x": 1.936134248900721,
+        "y": 1.9128215679513527
+      },
+      "nextControl": {
+        "x": 1.9291221613556924,
+        "y": 1.4128707397409563
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": {
+        "x": 1.9198835421653975,
+        "y": 6.331901987278464
+      },
+      "nextControl": {
+        "x": 1.9453728680910158,
+        "y": 6.8312518588753814
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 1.6628461538461545
+      },
+      "prevControl": {
+        "x": 1.9337461053582308,
+        "y": 1.9128436544318115
+      },
+      "nextControl": {
+        "x": 1.9315103048981825,
+        "y": 1.4128486532604976
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": {
+        "x": 1.8878500514131944,
+        "y": 6.335619778537727
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 10.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Shoot driving vertical.path

@@ -0,0 +1,118 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.956,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.707930530821583,
+        "y": 4.031008683647302
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 1.5499999999999998,
+        "y": 4.0
+      },
+      "nextControl": {
+        "x": 1.05,
+        "y": 4.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.9559102564102573,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.7118639603594925,
+        "y": 3.9457652751099213
+      },
+      "nextControl": {
+        "x": 3.199956552461022,
+        "y": 4.054234724890079
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 1.5434594129947146,
+        "y": 4.0568112156556175
+      },
+      "nextControl": {
+        "x": 1.0565405870052855,
+        "y": 3.9431887843443825
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.7532300973825357,
+        "y": 3.9599423520140395
+      },
+      "nextControl": {
+        "x": 3.2467699026174643,
+        "y": 4.040057647985961
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 1.526511041060335,
+        "y": 4.105795785727803
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 10.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -5,9 +5,14 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
 package frc4388.robot;
 
 import java.io.File;
+import java.util.Optional;
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -24,6 +29,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -129,6 +135,15 @@ public class RobotContainer {
             IntakeExtended,
             new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
         );
+
+        private Command RobotShootDriving = new SequentialCommandGroup(
+            new InstantCommand(() -> 
+                m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION)
+            ),
+            new WaitCommand(99) // stays on for the duration of the auto segment
+            ).finallyDo((interrupted) -> 
+             m_robotSwerveDrive.disableRotationOverride()
+        );
     
         private Command IntakeRetracted = new SequentialCommandGroup(
             new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
@@ -170,6 +185,7 @@ public class RobotContainer {
             NamedCommands.registerCommand("Robot Shoot", RobotShoot);
             // NamedCommands.registerCommand("Lidar Intake", LidarIntake);
             NamedCommands.registerCommand("Intake Extended", IntakeExtended);
+            NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
     
     
             DriverStation.silenceJoystickConnectionWarning(true);

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 = 171;
+  public static final int GIT_REVISION = 172;
-  public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7";
+  public static final String GIT_SHA = "69677d5ae9e63125a30857f5f7afa0756dfa9416";
-  public static final String GIT_DATE = "2026-03-19 18:32:02 MDT";
+  public static final String GIT_DATE = "2026-03-20 15:40:07 MDT";
   public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT";
+  public static final String BUILD_DATE = "2026-03-20 19:59:37 MDT";
-  public static final long BUILD_UNIX_TIME = 1774042307703L;
+  public static final long BUILD_UNIX_TIME = 1774058377112L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -9,6 +9,7 @@ import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 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;
@@ -114,12 +115,19 @@ public class Shooter extends SubsystemBase {
     public void periodic() {
         // FaultReporter.register(this); // TODO Implement fault reporter
 
+
         Logger.processInputs("Shooter", state);
         io.updateInputs(state);
 
 
         // Get robot positon and speeds
         ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
+        Translation2d robotSpeed = new Translation2d(
+            chassisSpeeds.vxMetersPerSecond, 
+            chassisSpeeds.vyMetersPerSecond
+        );
+        Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
+        Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle();
 
         Pose2d robotPose2d = m_SwerveDrive.getPose2d();
 
@@ -171,8 +179,10 @@ public class Shooter extends SubsystemBase {
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY);
                         break;
 
-                    case 0b001: // No errors and shoot button is pressed                        
+                    case 0b001: // No errors and shoot button is pressed     
+                    if (ang.getDegrees() < ShooterConstants.AIM_ANGLE.get()){       // robot is aimed at hub            
                         io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
+                    }
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY);
                         break;
 

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

@@ -2,8 +2,6 @@ package frc4388.robot.subsystems.shooter;
 
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
-import org.littletonrobotics.junction.Logger;
-
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
@@ -37,8 +35,8 @@ public class    ShooterConstants {
     public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
 
     
-    public static final double NEG_OFFSET = 8.;
+    public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.);
-    public static final double POS_OFFSET = 8.;
+    public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.);
 
     
     public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1);
@@ -47,6 +45,7 @@ public class    ShooterConstants {
     public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
     public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
 
+    public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 5);
     public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
 
     public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
@@ -65,12 +64,12 @@ public class    ShooterConstants {
         } else if (chassisXSpeed > 0){
             speed = 0.0593402*hubDistMeters*hubDistMeters +
             4.90561*hubDistMeters +
-            30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get();
+            30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
             
         } else { // Negative is closer to hub
             speed = 0.0593402*hubDistMeters*hubDistMeters +
             4.90561*hubDistMeters +
-            30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get();
+            30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
         }
         
         double max = SHOOTER_MAX_VELOCITY.get();

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

@@ -7,6 +7,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.Optional;
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -20,6 +21,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
 import com.pathplanner.lib.util.PathPlannerLogging;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -65,6 +67,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public Rotation2d orientRotTarget = new Rotation2d();
     public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
+    private final PIDController m_rotationOverridePID = new PIDController(
+        SwerveDriveConstants.PIDConstants.AIM_kP.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kI.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kD.get()
+    );
+    private boolean m_useRotationOverride = false;
+    private Translation2d m_rotationOverrideTarget = new Translation2d();
+
     /** Creates a new SwerveDrive. */
     public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
         // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
@@ -83,6 +93,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             // Handle exception as needed
             config = null;
         }
+
+        PPHolonomicDriveController driveController = new PPHolonomicDriveController(
+            new PIDConstants(5.0, 0.0, 0.0), // Translation PID
+            new PIDConstants(5.0, 0.0, 0.0)  // Rotation PID (used when override is OFF)
+        );
+        driveController.setRotationTargetOverride(() -> {
+            if (!m_useRotationOverride) return Optional.empty();
+            Rotation2d targetAngle = getPose2d()
+                .getTranslation()
+                .minus(m_rotationOverrideTarget)
+                .getAngle();
+            return Optional.of(targetAngle);
+        });
+
         // DoubleSupplier a = () -> 1.d;
         AutoBuilder.configure(
                 () -> {
@@ -94,11 +118,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
                 (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
                         .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
                                               // Also optionally outputs individual module feedforwards
-                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
+                driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...)
-                                                // holonomic drive trains
-                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
-                ),
                 config, // The robot configuration
                 () -> {
                     // Boolean supplier that controls when the path will be mirrored for the red
@@ -235,6 +255,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         
     }
 
+    
+
+    public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) {
+        Translation2d robotSpeed = new Translation2d(
+            chassisSpeeds.vxMetersPerSecond, 
+            chassisSpeeds.vyMetersPerSecond
+        );
+        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
+        Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
+
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+        .withVelocityX(chassisSpeeds.vxMetersPerSecond)
+        .withVelocityY(chassisSpeeds.vyMetersPerSecond)
+        .withTargetDirection(ang);
+        ctrl.HeadingController.setPID(
+        SwerveDriveConstants.PIDConstants.AIM_kP.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kI.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kD.get()
+        );
+        io.setControl(ctrl);
+    }
 
     public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
                                                                                                // reason to have a robot
@@ -408,33 +449,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         driveFieldAngle(leftStick, ang);
     }
 
-
-
-    public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, double ballVelocity, double distanceToHub) {
-
-        Translation2d robotSpeed = new Translation2d(
-            chassisSpeeds.vxMetersPerSecond, 
-            chassisSpeeds.vyMetersPerSecond
-        );
-
-        if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
-            double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
-            fieldPos =  new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
-            Logger.recordOutput("Offset Value", aimOffset);
-
-        }
-
-    
-
-        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);        
-
-        Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
-
-        Logger.recordOutput("Aim at Offset", fieldPos);
-
-        driveFieldAngle(leftStick, ang);
-    }
-
     public Pose2d getCurrentPose(){
         return state.currentPose;
     }
@@ -526,6 +540,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         softStop();
     }
 
+    public void enableRotationOverride(Translation2d fieldTarget) {
+        m_rotationOverrideTarget = fieldTarget;
+        m_useRotationOverride = true;
+    }
+
+    public void disableRotationOverride() {
+        m_useRotationOverride = false;
+    }
+
     @Override
     public void periodic() {
         // This method will be called once per scheduler run\
@@ -663,5 +686,4 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         return status;
     }
-}
+}
-