Lots

- shooting while moving corrected (vertical movements)
- reverse indexer before shooting
- defensive positions fixed values
- stalling motors led fixed (except shooter)

elastic-layout.json

@@ -8,10 +8,10 @@
         "layouts": [
           {
             "title": "Tag Processed",
-            "x": 384.0,
+            "x": 896.0,
             "y": 0.0,
             "width": 256.0,
-            "height": 256.0,
+            "height": 384.0,
             "type": "List Layout",
             "properties": {
               "label_position": "TOP"
@@ -55,27 +55,11 @@
           }
         ],
         "containers": [
-          {
-            "title": "MatchTime",
-            "x": 0.0,
-            "y": 0.0,
-            "width": 384.0,
-            "height": 256.0,
-            "type": "Match Time",
-            "properties": {
-              "topic": "/AdvantageKit/DriverStation/MatchTime",
-              "period": 0.06,
-              "data_type": "double",
-              "time_display_mode": "Minutes and Seconds",
-              "red_start_time": 15,
-              "yellow_start_time": 30
-            }
-          },
           {
             "title": "RetractedLimit",
-            "x": 0.0,
+            "x": 512.0,
-            "y": 384.0,
+            "y": 0.0,
-            "width": 128.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Boolean Box",
             "properties": {
@@ -90,9 +74,9 @@
           },
           {
             "title": "Auto Chooser",
-            "x": 1024.0,
+            "x": 896.0,
-            "y": 256.0,
+            "y": 384.0,
-            "width": 384.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "ComboBox Chooser",
             "properties": {
@@ -103,9 +87,9 @@
           },
           {
             "title": "Roller Percent Output",
-            "x": 0.0,
+            "x": 512.0,
             "y": 256.0,
-            "width": 256.0,
+            "width": 384.0,
             "height": 128.0,
             "type": "Number Slider",
             "properties": {
@@ -120,8 +104,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 128.0,
+            "x": 768.0,
-            "y": 384.0,
+            "y": 0.0,
             "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
@@ -134,8 +118,8 @@
           },
           {
             "title": "TRIM SHOOTER SPEED",
-            "x": 256.0,
+            "x": 512.0,
-            "y": 256.0,
+            "y": 384.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Number Slider",
@@ -151,8 +135,8 @@
           },
           {
             "title": "Mode",
-            "x": 256.0,
+            "x": 512.0,
-            "y": 384.0,
+            "y": 128.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Text Display",
@@ -164,30 +148,49 @@
             }
           },
           {
-            "title": "Time to rev",
+            "title": "IsActive",
-            "x": 768.0,
+            "x": 0.0,
-            "y": 128.0,
+            "y": 0.0,
-            "width": 256.0,
+            "width": 384.0,
-            "height": 128.0,
+            "height": 256.0,
-            "type": "Large Text Display",
+            "type": "Boolean Box",
             "properties": {
-              "topic": "/AdvantageKit/RealOutputs/Time to rev",
+              "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
               "period": 0.06,
-              "data_type": "double"
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
             }
           },
           {
-            "title": "Arm angle extended",
+            "title": "RemainingInShift",
-            "x": 1152.0,
+            "x": 0.0,
-            "y": 128.0,
+            "y": 256.0,
-            "width": 256.0,
+            "width": 384.0,
             "height": 128.0,
-            "type": "Text Display",
+            "type": "Match Time",
             "properties": {
-              "topic": "/SmartDashboard/Arm angle extended",
+              "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
               "period": 0.06,
               "data_type": "double",
-              "show_submit_button": true
+              "time_display_mode": "Minutes and Seconds",
+              "red_start_time": 15,
+              "yellow_start_time": 30
+            }
+          },
+          {
+            "title": "Phase",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
+              "period": 0.06,
+              "data_type": "string"
             }
           }
         ]
@@ -527,8 +530,8 @@
           },
           {
             "title": "Shooter Idle max current",
-            "x": 384.0,
+            "x": 1024.0,
-            "y": 512.0,
+            "y": 384.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -555,8 +558,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 640.0,
+            "x": 1024.0,
-            "y": 512.0,
+            "y": 256.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -575,12 +578,40 @@
       "grid_layout": {
         "layouts": [],
         "containers": [
+          {
+            "title": "Stalled Motor: ",
+            "x": 512.0,
+            "y": 128.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter idle % output",
+            "x": 128.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Shooter idle % output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
           {
             "title": "Auto Chooser",
-            "x": 0.0,
+            "x": 512.0,
-            "y": 0.0,
+            "y": 256.0,
-            "width": 640.0,
+            "width": 384.0,
-            "height": 768.0,
+            "height": 128.0,
             "type": "ComboBox Chooser",
             "properties": {
               "topic": "/SmartDashboard/Auto Chooser",

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

@@ -125,7 +125,7 @@ public class RobotContainer {
         // );
     
         private Command RobotRev = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter),
+            new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
             IntakeExtended,
             new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
         );
@@ -149,7 +149,7 @@ public class RobotContainer {
         
         public RobotContainer() {
             
-            configureButtonBindings();
+            configureSINGLEBindings();
             
             // Called on first robot enable
             DeferredBlock.addBlock(() -> {
@@ -313,7 +313,7 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Idle);
                 m_robotIntake.rollerStop();
-                m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds());
+                m_robotShooter.spinUpShooting();
             }))
             .onFalse(new InstantCommand(() -> {
                 m_robotShooter.spinUpIdle();
@@ -350,16 +350,125 @@ public class RobotContainer {
         //         m_robotClimber.toggleDeployed();
         //     }));
 
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
+
+   }
+
+
+   private void configureSINGLEBindings() {
+            
+            //Driver controls
+            new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+    
+            new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+                .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+            
+            new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+    
+    
+    
+                
+            // TEST-> the driver is holding the left trigger, drive slow and rotation up 
+            // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+            //     .onTrue(new InstantCommand(() -> {
+            //         m_robotSwerveDrive.setToSlow();
+            //         m_robotSwerveDrive.shiftUpRot();
+            //     }))
+            //     .onFalse(new InstantCommand(() -> {
+            //         m_robotSwerveDrive.setToFast();
+            //         m_robotSwerveDrive.shiftDownRot();
+            // }));
+    
+            //TEST - > X positino on wheels
+            new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+                .whileTrue(new RunCommand(() -> {
+                m_robotSwerveDrive.defenseXPosition();
+            }, m_robotSwerveDrive))
+                .onFalse(new InstantCommand(() -> {
+                m_robotSwerveDrive.stopDefenseXPosition();
+            }));
+
+            //TEST - > PID positinon
+            new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {
-                m_robotShooter.indexerStalled();
+                    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().getLeftTriggerAxis() >= 0.5)
+            .onTrue(new InstantCommand(() -> {
+                    m_robotSwerveDrive.setToSlow();
+            }))
+            .whileTrue(new RunCommand(() -> {
+                m_robotSwerveDrive.driveFacingPosition(
+                    getDeadbandedDriverController().getLeft(),
+                    FieldPositions.HUB_POSITION,
+                    ShooterConstants.AIM_LEAD_TIME.get()
+                    );
+                }, m_robotSwerveDrive)
+            );
+
+        // D-PAD fine alignment
+        new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
+            .whileTrue(new RunCommand(
+                () -> m_robotSwerveDrive.driveFine(
+                    new Translation2d(
+                        1, 
+                        Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
+                    ), 
+                    getDeadbandedDriverController().getRight(), 0.15
+                ), m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+
+
+        //allow shooting with right trigger
+        new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
+            .onTrue(new InstantCommand(() -> {
+                m_robotShooter.allowShooting();
+            })).onFalse(new InstantCommand(() -> {
+                m_robotShooter.denyShooting();
+            }));
+        
+
+
+        //set shooter ready (rev) with left trigger hold
+        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+                m_robotIntake.rollerStop();
+                m_robotShooter.spinUpShooting();
             }))
             .onFalse(new InstantCommand(() -> {
                 m_robotShooter.spinUpIdle();
             }));
 
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Extending);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+            }));
 
-   }
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Retracting);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+            }));
+
+        }
 
 //.onTrue(new InstantCommand(()  -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
 

src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java

@@ -34,7 +34,7 @@ public class StayInPosition extends PID {
         //Same here
           translationY = 0.2;
         }
-        if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) {
+        if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) {
             driveTranslation = new Translation2d();
         } else {
             //TODO: Tweak for best corrector against another bot
@@ -42,8 +42,6 @@ public class StayInPosition extends PID {
         }
 
         drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
-        // If above doesn't work
-        //drive.driveFieldAngle(driveTranslation, targetPose.getRotation());
     }
 
     @Override

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 = 170;
+  public static final int GIT_REVISION = 171;
-  public static final String GIT_SHA = "917aaa774650423095bb33033fe57b1eb1c12a1a";
+  public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7";
-  public static final String GIT_DATE = "2026-03-19 15:46:28 MDT";
+  public static final String GIT_DATE = "2026-03-19 18:32:02 MDT";
   public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT";
+  public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT";
-  public static final long BUILD_UNIX_TIME = 1773965797063L;
+  public static final long BUILD_UNIX_TIME = 1774042307703L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -13,12 +13,14 @@ 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.units.measure.Current;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants;
 import frc4388.robot.subsystems.intake.Intake;
 import frc4388.robot.subsystems.led.LED;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.utility.compute.FieldPositions;
+import frc4388.utility.compute.TimesNegativeOne;
 
 public class Shooter extends SubsystemBase {
     public ShooterIO io;
@@ -34,6 +36,8 @@ public class Shooter extends SubsystemBase {
     public double distanceToHub = 5;
     public double chassisXSpeed = 0;
 
+
+
     public Shooter(
         ShooterIO io,
         SwerveDrive swerveDrive,
@@ -59,15 +63,13 @@ public class Shooter extends SubsystemBase {
     public enum ShooterMode {
         Shooting,
         ManualShoot,
-        Idle,
+        Idle
-        IndexerStall
     }
 
     private ShooterMode mode = ShooterMode.Idle;
     private boolean shooterButtonReady = false;
 
-    public void spinUpShooting(double chassisXSpeed) {
+    public void spinUpShooting() {
-        this.chassisXSpeed = chassisXSpeed;
         this.mode = ShooterMode.Shooting;
     }
 
@@ -87,10 +89,6 @@ public class Shooter extends SubsystemBase {
         shooterButtonReady = true;
     }   
 
-    public void indexerStalled() {
-        mode = ShooterMode.IndexerStall;
-    }
-
     public void denyShooting() {
         shooterButtonReady = false;
     }
@@ -122,26 +120,20 @@ public class Shooter extends SubsystemBase {
 
         // Get robot positon and speeds
         ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
-        double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2));
-        double AngSpeed  = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI));
 
         Pose2d robotPose2d = m_SwerveDrive.getPose2d();
 
+        if (TimesNegativeOne.isRed) {
+            chassisXSpeed = chassisSpeeds.vxMetersPerSecond;
+        } else {
+            chassisXSpeed = -chassisSpeeds.vxMetersPerSecond;
+        }
 
-        // Calculate aim lead
+        Translation2d fieldPos = robotPose2d.getTranslation();
-        // 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(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
-
         // Get the robot's aim distance to hub
-        distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
+        distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm());
 
-        //Center of hub to cameras in inches
+        //Center of hub to cameras in meters
         Logger.recordOutput("Hub Dist", distanceToHub);
 
         boolean driverError = 
@@ -158,14 +150,14 @@ public class Shooter extends SubsystemBase {
 
         //revtime calculations
         // double shooterAcceleration = 
-        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
+        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond);
         double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
         // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
         Logger.recordOutput("Time to rev", revTime);
 
         switch (mode) {
             case Shooting:
-                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
+                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
 
                 int bitmask = (
                     (shooterButtonReady ? 1 : 0) +
@@ -175,18 +167,18 @@ public class Shooter extends SubsystemBase {
 
                 switch (bitmask) {
                     case 0b000: // No errors but button is not pressed
-                        io.setIndexerOutput(state, 0);
+                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
                         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                        
                         io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY);
                         break;
 
                     case 0b010: // Bad shooter velocity, button is not pressed
                     case 0b011: // Bad shooter velocty, button is pressed
-                        io.setIndexerOutput(state, 0);
+                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
                         m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
                         break;
 
@@ -253,11 +245,6 @@ public class Shooter extends SubsystemBase {
                 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

@@ -2,6 +2,8 @@ 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;
@@ -31,10 +33,14 @@ 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.4);
+    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2);
     public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
 
     
+    public static final double NEG_OFFSET = 8.;
+    public static final double POS_OFFSET = 8.;
+
+    
     public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1);
 
     // Shoot mode tolerances
@@ -49,21 +55,23 @@ public class    ShooterConstants {
     public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); 
 
     // 
-    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
+    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
-        // Model derived from points
+            double speed = 0;
-        // double speed = 
+
-        //     1.11576*hubDistMeters*hubDistMeters +
+        if (Math.abs(chassisXSpeed) < 0.1){
-        //     0.318464*hubDistMeters +
+            speed = 0.0593402*hubDistMeters*hubDistMeters +
-        //     30.6293;
-        double speed = 
-            0.0593402*hubDistMeters*hubDistMeters +
             4.90561*hubDistMeters +
             30.35696 + MODEL_TRIM.get();
-
+        } else if (chassisXSpeed > 0){
-        // double speed =
+            speed = 0.0593402*hubDistMeters*hubDistMeters +
-        //     0.00610938*hubDistMeters*hubDistMeters
+            4.90561*hubDistMeters +
-        //     5.65235*hubDistMeters +
+            30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get();
-        //     22.82825;
+            
+        } else { // Negative is closer to hub
+            speed = 0.0593402*hubDistMeters*hubDistMeters +
+            4.90561*hubDistMeters +
+            30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get();
+        }
         
         double max = SHOOTER_MAX_VELOCITY.get();
 

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

@@ -55,21 +55,24 @@ public class ShooterReal implements ShooterIO {
     }
     
     @Override
-    public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {        
+    public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {    
-        if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
+            motorStall = "";
-            if (!m_shooterStalling) {
+        // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
-                m_shooterStalling = true;
+        //     if (!m_shooterStalling) {
-                m_stallTimerShooter.restart();
+        //         m_shooterStalling = true;
-            }
+        //         m_stallTimerShooter.restart();
-            if (m_stallTimerShooter.hasElapsed(5.0)) {
+        //     }
-                m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+        //     if (m_stallTimerShooter.hasElapsed(5.0)) {
-                motorStall = "Shooter Stalled";
+        //         m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
-                System.out.println(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
+        //         motorStall = "Shooter Stalled";
-            }
+        //         System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))));
-        } else {
+        //         System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)));
-            m_shooterStalling = false;
+        //         System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
-            m_stallTimerShooter.reset();
+        //     }
-        }
+        // } else {
+        //     m_shooterStalling = false;
+        //     m_stallTimerShooter.reset();
+        // }
 
         if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) {
             if (!m_indexerStalling) {
@@ -79,7 +82,7 @@ public class ShooterReal implements ShooterIO {
             if (m_stallTimerIndexer.hasElapsed(5.0)) {
                 m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
                 motorStall = "Indexer Stalled";
-                System.out.println(Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput));
+                System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)));
             }
         } else {
             m_indexerStalling = false;
@@ -94,7 +97,7 @@ public class ShooterReal implements ShooterIO {
             if (m_stallTimerRoller.hasElapsed(5.0)) {
                 m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
                 motorStall = "Roller Stalled";
-                System.out.println(Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()));
+                System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())));
             }
         } else {
             m_rollerStalling = false;