Update gear ratios

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

@@ -233,20 +233,20 @@ public class RobotContainer {
     
             //TEST - > Defense: X position on wheels and swerve drive pid on position
             new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            //     .whileTrue(new RunCommand(() -> {
+                .whileTrue(new RunCommand(() -> {
-            //     m_robotSwerveDrive.defenseXPosition();
+                m_robotSwerveDrive.defenseXPosition();
-            // }, m_robotSwerveDrive))
-            //     .onFalse(new InstantCommand(() -> {
-            //     m_robotSwerveDrive.stopDefenseXPosition();
-
-            .onTrue(new InstantCommand(() -> {
-                    currentPose = m_robotSwerveDrive.getCurrentPose();
-            }))
-            .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();
                 
             }));
 
@@ -352,6 +352,14 @@ public class RobotContainer {
         //         m_robotClimber.toggleDeployed();
         //     }));
 
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotShooter.indexerStalled();
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotShooter.spinUpIdle();
+            }));
+
 
    }
 

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 = 166;
+  public static final int GIT_REVISION = 167;
-  public static final String GIT_SHA = "86b36a687e0338ed3ae5c6d035fb0f5b7404c27b";
+  public static final String GIT_SHA = "85fe11c8bdd51d8671e98047f88a2d42276f7094";
-  public static final String GIT_DATE = "2026-03-16 16:13:53 MDT";
+  public static final String GIT_DATE = "2026-03-18 13:39:27 MDT";
   public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-18 13:27:40 MDT";
+  public static final String BUILD_DATE = "2026-03-18 16:02:33 MDT";
-  public static final long BUILD_UNIX_TIME = 1773862060483L;
+  public static final long BUILD_UNIX_TIME = 1773871353359L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -117,6 +117,11 @@ public final class Constants {
         // Operator ready to shoot, but the driver conditions are bad
         public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
 
+        // LED color for when the indexer was stuck on ball and going in reverse
+        public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET;
+
+        // LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled
+        public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED;
 
         public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
         // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -45,6 +45,13 @@ public class Intake extends SubsystemBase {
         io.setRollerOutput(state, 0);
     }
 
+    public double getRollerTarget() {
+        return state.rollerTargetOutput;
+    }
+
+    public double getRollerSpeed() {
+        return state.rollerOutput;
+    }
 
     // public enum FieldZone {
     //     // The robot should aim at the hub

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

@@ -59,7 +59,8 @@ public class Shooter extends SubsystemBase {
     public enum ShooterMode {
         Shooting,
         ManualShoot,
-        Idle
+        Idle,
+        IndexerStall
     }
 
     private ShooterMode mode = ShooterMode.Idle;
@@ -83,6 +84,10 @@ public class Shooter extends SubsystemBase {
 
     public void allowShooting() {
         shooterButtonReady = true;
+    }   
+
+    public void indexerStalled() {
+        mode = ShooterMode.IndexerStall;
     }
 
     public void denyShooting() {
@@ -169,7 +174,7 @@ public class Shooter extends SubsystemBase {
 
                 switch (bitmask) {
                     case 0b000: // No errors but button is not pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY);
                         break;
 
@@ -180,19 +185,19 @@ public class Shooter extends SubsystemBase {
 
                     case 0b010: // Bad shooter velocity, button is not pressed
                     case 0b011: // Bad shooter velocty, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
                         break;
 
                     case 0b100: // Driver error, button is not pressed
                     case 0b101: // Driver error, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
                         break;
 
                     case 0b110: // Driver error, bad shooter vel, button is not pressed
                     case 0b111: // Driver error, bad shooter vel, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
                         break;
                 }
@@ -207,7 +212,7 @@ public class Shooter extends SubsystemBase {
 
                 switch (bitmask2) {
                     case 0b000: // No errors but button is not pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
                         break;
 
@@ -218,7 +223,7 @@ public class Shooter extends SubsystemBase {
 
                     case 0b010: // Bad shooter velocity, button is not pressed
                     case 0b011: // Bad shooter velocty, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
                         break;
 
@@ -244,12 +249,16 @@ public class Shooter extends SubsystemBase {
                     // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), 
                     // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
                 );
-                io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                io.setIndexerOutput(state, 0);
                 m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
                 break;
-            
+            case IndexerStall:
-
+                io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get()));
+                io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                m_robotLED.setMode(Constants.LEDConstants.INDEXER_REVERSE);
+                break;
             }
-
+            
+        io.motorStalled(state, m_Intake, m_robotLED);
     }
 }

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

@@ -15,7 +15,7 @@ import frc4388.utility.status.CanDevice;
 public class    ShooterConstants {
     // Motor conversions
     
-    public static final double SHOOTERMOTOR_GEAR_RATIO = 1.29; // TODO: supposed to be 9 rotations in to 7 out
+    public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out    -- 0.77 or 1.29
     public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31 
     public static final double T_CONSTANT = 2;
     public static final double SHOOTER_RADIUS = 2/39.37;
@@ -31,8 +31,8 @@ public class    ShooterConstants {
     // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
 
     public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
-    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
+    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.4);
-    public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5);
+    public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
 
     
     public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
@@ -56,8 +56,9 @@ public class    ShooterConstants {
         //     0.318464*hubDistMeters +
         //     30.6293;
         double speed = 
-            5.6939*hubDistMeters +
+            0.0593402*hubDistMeters*hubDistMeters +
-            22.76545 + MODEL_TRIM.get();
+            4.90561*hubDistMeters +
+            30.35696 + MODEL_TRIM.get();
 
         // double speed =
         //     0.00610938*hubDistMeters*hubDistMeters
@@ -81,8 +82,8 @@ public class    ShooterConstants {
     // Motor Configuration
     public static Slot0Configs SHOOTER_PID = new Slot0Configs()
         .withKV(0.0)
-        .withKP(0.08)
+        .withKP(0.05)
-        .withKI(0.15)
+        .withKI(0.20)
         .withKD(0.0);
 
     

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

@@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.led.LED;
 
 public interface ShooterIO {
     @AutoLog
@@ -49,6 +51,7 @@ public interface ShooterIO {
 
     public default void updateInputs(ShooterState state) {}
 
+    public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {}
     
     public default void updateGains() {}
 }

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

@@ -8,6 +8,10 @@ import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.Timer;
+import frc4388.robot.constants.Constants;
+import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.led.LED;
 
 public class ShooterReal implements ShooterIO {
 
@@ -19,6 +23,12 @@ public class ShooterReal implements ShooterIO {
     VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
     // VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
 
+    private final Timer m_stallTimerShooter = new Timer();
+    private final Timer m_stallTimerIndexer = new Timer();
+    private final Timer m_stallTimerRoller = new Timer();
+    private boolean m_shooterStalling = false;
+    private boolean m_indexerStalling = false;
+    private boolean m_rollerStalling = false;
 
     public ShooterReal(
          TalonFX shooter1Motor,
@@ -41,6 +51,50 @@ public class ShooterReal implements ShooterIO {
         // m_indexerVelocity.Slot = 0;
     }
     
+    @Override
+    public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {
+        
+        // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 20) {
+        //     if (!m_shooterStalling) {
+        //         m_shooterStalling = true;
+        //         m_stallTimerShooter.restart();
+        //     }
+        //     if (m_stallTimerShooter.hasElapsed(5.0)) {
+        //         m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+        //     }
+        // } else {
+        //     m_shooterStalling = false;
+        //     m_stallTimerShooter.stop();
+        // }
+
+        // if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.2) {
+        //     if (!m_indexerStalling) {
+        //         m_indexerStalling = true;
+        //         m_stallTimerIndexer.restart();
+        //     }
+        //     if (m_stallTimerIndexer.hasElapsed(5.0)) {
+        //         m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+        //     }
+        // } else {
+        //     m_indexerStalling = false;
+        //     m_stallTimerIndexer.stop();
+        // }
+
+        // if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.2) {
+        //     if (!m_rollerStalling) {
+        //         m_rollerStalling = true;
+        //         m_stallTimerRoller.restart();
+        //     }
+        //     if (m_stallTimerRoller.hasElapsed(5.0)) {
+        //         m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+        //     }
+        // } else {
+        //     m_rollerStalling = false;
+        //     m_stallTimerRoller.stop();
+        // }
+        
+    }
+    
     @Override
     public void setShooterVelocity(ShooterState state, AngularVelocity target) {
         state.motor1TargetVelocity = target;

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

@@ -410,12 +410,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         );
 
         if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
-            double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity));
+            double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
-            double aimOffsetComplext = (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)));
-
             fieldPos =  new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
             Logger.recordOutput("Offset Simple", aimOffset);
-            Logger.recordOutput("Offset Complex", aimOffsetComplext);
 
         }
 

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

@@ -73,6 +73,8 @@ 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 final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
 
     public static final class ModuleSpecificConstants { //2026