Merge branch 'reveal-night' into AutoTesting

src/main/deploy/pathplanner/paths/Taxi.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
+        "x": 3.62702380952381,
-        "y": 7.0
+        "y": 6.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
+        "x": 4.627023809523808,
-        "y": 7.0
+        "y": 6.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.9612559241706156,
+        "x": 2.180142857142858,
-        "y": 7.0
+        "y": 6.0
       },
       "prevControl": {
-        "x": 1.9612559241706156,
+        "x": 1.180142857142858,
-        "y": 7.0
+        "y": 6.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.2,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 178.89829388479367
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 179.95980050207663
   },
   "useDefaultConstraints": false
 }

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

@@ -267,29 +267,28 @@ public class RobotContainer {
 
         new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5)
             .onTrue(new InstantCommand(() -> {
-                m_robotShooter.setShooterNotReady();
+                m_robotShooter.spinUpIdle();
             }));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
             .onTrue(new InstantCommand(() -> {
-                m_robotShooter.setShooterReady();
+                m_robotShooter.spinUpShooting();
                 m_robotIntake.setMode(IntakeMode.Idle);
             }));
 
         new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
             .onTrue(new InstantCommand(() -> {
-                m_robotShooter.setShooterReadyFeeder();
+                m_robotShooter.spinUpFeeding();
             }));
 
         
 
 
-
         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> {
-                m_robotShooter.setShooterShoot();
+                m_robotShooter.allowShooting();
             })).onFalse(new InstantCommand(() -> {
-                m_robotShooter.setShooterNOTShoot();
+                m_robotShooter.denyShooting();
             }));
 
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)

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 = 96;
+  public static final int GIT_REVISION = 85;
-  public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1";
+  public static final String GIT_SHA = "3197a3dce03bf34c001ef4009de3e7d98a181536";
-  public static final String GIT_DATE = "2026-02-25 18:59:11 MST";
+  public static final String GIT_DATE = "2026-02-24 22:17:58 MST";
-  public static final String GIT_BRANCH = "AutoTesting";
+  public static final String GIT_BRANCH = "reveal-night";
-  public static final String BUILD_DATE = "2026-02-25 20:19:51 MST";
+  public static final String BUILD_DATE = "2026-02-25 16:32:13 MST";
-  public static final long BUILD_UNIX_TIME = 1772075991136L;
+  public static final long BUILD_UNIX_TIME = 1772062333884L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -119,7 +119,7 @@ public final class Constants {
 
 
         public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
-        public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
+        // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;
     }
 
     public static final class OIConstants {

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

@@ -1,5 +1,6 @@
 package frc4388.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Rotation;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
@@ -7,7 +8,10 @@ import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Pose2d;
+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.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants;
 import frc4388.robot.subsystems.LED;
@@ -50,43 +54,36 @@ public class Shooter extends SubsystemBase {
 
     
     public enum ShooterMode {
-        // Shooter is actively shooting
         Shooting,
-        // Shooter is going to fire soon
+        Feeding,
-        Ready,
+        Idle
-
-        ShootingFeeder,
-        ReadyFeeder,
-
-        // Not ready to shoot
-        NotReady,
     }
 
-    private ShooterMode mode = ShooterMode.NotReady;
+    private ShooterMode mode = ShooterMode.Idle;
     private boolean shooterButtonReady = false;
 
-    public void setShooterReady() {
+    public void spinUpShooting() {
-        this.mode = ShooterMode.Ready;
+        this.mode = ShooterMode.Shooting;
+    }
+
+    public void spinUpFeeding() {
+        this.mode = ShooterMode.Feeding;
+    }
+    
+    public void spinUpIdle() {
+        this.mode = ShooterMode.Idle;
     }
 
 
-    public void setShooterReadyFeeder() {
+    public void allowShooting() {
-        this.mode = ShooterMode.ReadyFeeder;
-    }
-    
-    public void setShooterNotReady() {
-        this.mode = ShooterMode.NotReady;
-    }
-
-
-    public void setShooterShoot() {
         shooterButtonReady = true;
     }
 
-    public void setShooterNOTShoot() {
+    public void denyShooting() {
         shooterButtonReady = false;
     }
 
+
     @AutoLogOutput
     public ShooterMode getMode() {
         return mode;
@@ -101,111 +98,133 @@ public class Shooter extends SubsystemBase {
         io.updateInputs(state);
 
 
-        ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds;
+        // Get robot positon and speeds
-        double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2));
+        ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
-        double AngSpeed  = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI));
+        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();
-        //
+
-        double distanceToHub = (robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm());
+
+        // Calculate aim lead
+        // 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
+        double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
+
         //Center of hub to cameras in inches
         Logger.recordOutput("Hub Dist", distanceToHub);
 
-
+        boolean driverError = 
-        if(this.mode != ShooterMode.NotReady) {
+            // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
-            // TODO: get if the robot is within the angle of the hub
+            // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
-
+            distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | 
-            boolean driverError = 
+            distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
-                // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
-                // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
-                distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | 
-                distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
-
-            double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
-            double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2;
-
-            boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
-        //     boolean intakeBad = m_Intake.getMode() == IntxakeMode.Extended;
-
-
-            int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (this.mode == ShooterMode.ReadyFeeder ? 4 : 0);
-            switch (bitmask) {
-                case 0b000: // No Errors
-                    m_robotLED.setMode(Constants.LEDConstants.OPREADY);
-                    break;
-                case 0b001: // No op err, yes driver err
-                    m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
-                    break;
-
-                case 0b010:
-                case 0b110: // Bad flywheel, no driver err
-                    m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
-                    break;
-
-                case 0b011:
-                case 0b111: // Bad flywheel, yes driver err
-                    m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
-                    break;
-
-                case 0b100:
-                    m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
-                    break;
-                case 0b101:
-                    m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED_BADPHYS);
-                    break;
-            }
-
-        //     // We set the shooter mode to ready if there are no errors
-            mode = (
-                bitmask == 0 ?
-                ShooterMode.Shooting :
-                ShooterMode.Ready
-            );
-
-        } else {
-            m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
-
-        }
 
 
+        double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
+        double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2;
 
+        boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
 
         switch (mode) {
             case Shooting:
                 io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
 
-                if(shooterButtonReady) {
+                int bitmask = (
-                    io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
+                    (shooterButtonReady ? 1 : 0) +
-                } else {
+                    (badShooterVelocity ? 2 : 0) +
-                    io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                    (driverError ? 4 : 0)
+                );
+
+                switch (bitmask) {
+                    case 0b000: // No errors but button is not pressed
+                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        m_robotLED.setMode(Constants.LEDConstants.OPREADY);
+                        break;
+
+                    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, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        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());
+                        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());
+                        m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
+                        break;
                 }
                 break;
-
+            case Feeding:
-            case Ready:
-                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
-                io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
-                break;
-
-            case ShootingFeeder:
                 io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get()));
 
-                if(shooterButtonReady) {
+                int bitmask2 = (
-                    io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
+                    (shooterButtonReady ? 1 : 0) +
-                } else {
+                    (badShooterVelocity ? 2 : 0)
-                    io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                );
+
+                switch (bitmask2) {
+                    case 0b000: // No errors but button is not pressed
+                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
+                        break;
+
+                    case 0b001: // No errors and shoot button is pressed
+                        io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
+                        m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
+                        break;
+
+                    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());
+                        m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
+                        break;
+
+                    // case 0b100: // Driver error, button is not pressed
+                    // case 0b101: // Driver error, button is pressed
+                    //     m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
+                    //     io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                    //     break;
+
+                    // case 0b110: // Driver error, bad shooter vel, button is not pressed
+                    // case 0b111: // Driver error, bad shooter vel, button is pressed
+                    //     m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
+                    //     io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                    //     break;
                 }
-                break;
 
-            case ReadyFeeder:
-                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
-                io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
                 break;
+            case Idle:
 
-            case NotReady:
+                io.setShooterCurrentLimitSpeed(
-                io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
+                    state, 
+                    ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get()
+                    // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), 
+                    // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
+                );
                 io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
                 break;
-        }
+            }
 
     }
 }

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

@@ -18,14 +18,18 @@ public class    ShooterConstants {
     public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
     public static final double INDEXER_GEAR_RATIO = 1.;
 
-    // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40);
     public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
-    public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
     public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
+    // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
+    
+    public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3);
+    // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
+    // 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 AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
 
     // Shoot mode tolerances

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

@@ -38,6 +38,12 @@ public interface ShooterIO {
     // public default void setShooterAngle(ShooterState state, Angle angle) {}
     // public default void setShooterPitch(ShooterState state, Angle angle) {}
     public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {}
+    public default void setShooterCurrentLimitSpeed(
+        ShooterState state, 
+        double percentOutput
+        // Current currentLimit,
+        // AngularVelocity targetVelocity
+    ) {}
     // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {}
     public default void setIndexerOutput(ShooterState state, double percentOutput) {}
 

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

@@ -1,9 +1,13 @@
 package frc4388.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
 
 public class ShooterReal implements ShooterIO {
 
@@ -54,6 +58,31 @@ public class ShooterReal implements ShooterIO {
         m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps));
     }
 
+    @Override
+    public void setShooterCurrentLimitSpeed(
+        ShooterState state, 
+        double percentOutput
+        // Current currentLimit,
+        // AngularVelocity targetVelocity
+    ) {
+        // state.motor1TargetVelocity = targetVelocity;
+        // state.motor2TargetVelocity = targetVelocity;
+
+        // double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps));
+        // double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2;
+
+        // if(
+        //     Math.abs(currentLimit.in(Amps)) > current &&
+        //     Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
+        // ) {
+            m_shooter1Motor.set(percentOutput);
+            m_shooter2Motor.set(percentOutput);
+        // } else {
+        //     m_shooter1Motor.set(0);
+        //     m_shooter2Motor.set(0);
+        // }
+    }
+
     @Override
     public void setIndexerOutput(ShooterState state, double percentOutput) {
         state.indexerTargetOutput = percentOutput;

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems.swerve;
 
+import static edu.wpi.first.units.Units.Rotations;
+
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -245,6 +247,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
+        if(!TimesNegativeOne.isRed) {
+            leftStick.rotateBy(new Rotation2d(Math.PI/2.));
+        }
+
         io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)

src/main/java/frc4388/utility/compute/FieldPositions.java

@@ -6,7 +6,7 @@ public class FieldPositions {
     // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0);
     // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0);
     public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3);
-    public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3);
+    public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534);// + 0.3);
 
 
     // We set the default position to one just in case it doesn't update