WIP

Tweaked shooter offset model and new progress with X position

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

@@ -231,18 +231,27 @@ public class RobotContainer {
                     m_robotSwerveDrive.shiftDownRot();
             }));
     
-            //TEST - > Swerve drive pid on position
+            //TEST - > Defense: X position on wheels and swerve drive pid on position
             new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-                .onTrue(new InstantCommand(() -> {
+                .whileTrue(new RunCommand(() -> {
-                    currentPose = m_robotSwerveDrive.getCurrentPose();
+                m_robotSwerveDrive.defenseXPosition();
-            }))
-            .whileTrue(new RunCommand(() -> {
-                m_stayInPosition.goToTargetPose(currentPose);
             }, m_robotSwerveDrive))
-            .onFalse(new InstantCommand(() -> {
+                .onFalse(new InstantCommand(() -> {
-                m_robotSwerveDrive.softStop();
+                m_robotSwerveDrive.stopDefenseXPosition();
+
+            // .onTrue(new InstantCommand(() -> {
+            //         currentPose = m_robotSwerveDrive.getCurrentPose();
+            // }))
+            // .whileTrue(new RunCommand(() -> {
+            //     m_stayInPosition.goToTargetPose(currentPose);
+            // }, m_robotSwerveDrive))
+            // .onFalse(new InstantCommand(() -> {
+            //     m_robotSwerveDrive.softStop();
+            // }));
             }));
 
+            
+
         // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
         new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
             .whileTrue(new RunCommand(() -> m_robotSwerveDrive.driveFacingPosition(

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 = 157;
+  public static final int GIT_REVISION = 158;
-  public static final String GIT_SHA = "2de8c605970a55114d3a66ebfb26160df3e0aa50";
+  public static final String GIT_SHA = "07ec609b019fd8db439e14beb74c50fdbf75a091";
-  public static final String GIT_DATE = "2026-03-12 18:35:44 MDT";
+  public static final String GIT_DATE = "2026-03-14 15:41:27 MDT";
   public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-14 15:24:12 MDT";
+  public static final String BUILD_DATE = "2026-03-14 17:27:47 MDT";
-  public static final long BUILD_UNIX_TIME = 1773523452704L;
+  public static final long BUILD_UNIX_TIME = 1773530867415L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -89,8 +89,9 @@ public class Shooter extends SubsystemBase {
         shooterButtonReady = false;
     }
 
-    public AngularVelocity getBallVelocity() {
+    public double getBallVelocity() {
-        return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond)));
+        return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI;
+        //Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS)
     }
 
     @AutoLogOutput

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

@@ -18,7 +18,8 @@ public class    ShooterConstants {
     public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
     public static final double INDEXER_GEAR_RATIO = 1.;
     public static final double T_CONSTANT = 2;
-
+    public static final double SHOOTER_RADIUS = 2/39.37;
+    public static final double INDEXER_RADIUS = 0.625/39.37;
     public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
     public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42);
     // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);

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

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

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

@@ -87,6 +87,9 @@ public class ShooterReal implements ShooterIO {
     public void setIndexerOutput(ShooterState state, double percentOutput) {
         state.indexerTargetOutput = percentOutput;
         m_indexerMotor.set(percentOutput);
+        if (state.indexerTargetOutput - state.indexerOutput > 0.05){
+            state.indexerForwardVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO);
+        }
     }
 
 
@@ -95,7 +98,6 @@ 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

@@ -150,14 +150,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public void setInitalPose(Pose2d startingAutoPose){
         initalPose2d = startingAutoPose;
     }
-    // MIRA public void setOdoPose(Pose2d pose) {
-    //     if (pose == null) return;
-    //     io.tareEverything();
-    //     initalPose2d = pose;
-    //     io.resetPose(pose);
-    //     robotKnowsWhereItIs = true;
-    //     rotTarget = pose.getRotation().getDegrees();
-    // }
 
 
     // public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -378,20 +370,41 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         driveFieldAngle(leftStick, ang);
     }
 
+    public void defenseXPosition(){
+        io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0));
+        io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0));
+        io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0));
+        io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0));
+    }
 
-    public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) {
+    public void stopDefenseXPosition(){
+            io.restoreSteerOffsets();
+    }
+
+    public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
+        // Calculate the angle between the current position and the lead position
+        //Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle();
+        Rotation2d ang = new Rotation2d(0,1);
+        System.out.println(ang);
+
+        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.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){
+        if (ballVelocity > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){
-            double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity.in(RotationsPerSecond)));
+            double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity));
             fieldPos =  new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
             Logger.recordOutput("Aim Offset", aimOffset);
 
-            // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity.in(RotationsPerSecond)) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub)));
+            // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub)));
             // Logger.recordOutput("Aim Offset", aimOffset);
         }
 

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

@@ -75,9 +75,10 @@ public final class SwerveDriveConstants {
 
     // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
 
-    private static final class ModuleSpecificConstants { //2025
+    public static final class ModuleSpecificConstants { //2026
         //Front Left
-        private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
+        public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
+        public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
         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;
@@ -85,7 +86,8 @@ 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.465332+0.3+0.003174-0.0103);
+        public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103);
+        public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103);
         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;
@@ -93,7 +95,8 @@ 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.023438+0.5+0.0168-0.00562);
+        public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562);
+        public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562);
         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;
@@ -101,7 +104,8 @@ 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.029541+0.05-0.002197-0.00366);
+        public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366);
+        public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366);
         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;

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

@@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.CanDevice;
 
 public interface SwerveIO {
     @AutoLog
@@ -25,6 +27,10 @@ public interface SwerveIO {
 
     public default void tareEverything() {}
 
+    public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {}
+
+    public default void restoreSteerOffsets(){}
+
     public default void resetPose(Pose2d pose) {}
 
     public default void addVisionMeasurement(List<PoseObservation> poses) {}

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

@@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.CanDevice;
 
 public class SwerveReal implements SwerveIO {
     SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
@@ -55,6 +57,15 @@ public class SwerveReal implements SwerveIO {
         }
     }
 
+    @Override
+    public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {
+    }
+
+    @Override
+    public void restoreSteerOffsets() {
+
+    }
+    
     @Override
     public void setLimits(double limitInAmps) {
         for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {

src/main/java/frc4388/utility/status/CanDevice.java

@@ -49,5 +49,10 @@ public class CanDevice {
         return s;
     }
 
+
+    public Object getId() {
+        return id;        
+    }
+
     
 }