Implement field positions, tolerance, LED states

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

@@ -40,6 +40,7 @@ import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.compute.FieldPositions;
 import frc4388.utility.compute.TimesNegativeOne;
 import frc4388.utility.controller.DeadbandedXboxController;
 // Autos
@@ -112,24 +113,29 @@ public class RobotContainer {
         
         configureButtonBindings();
         
-        DeferredBlock.addBlock(() -> { // Called on first robot enable
+        // Called on first robot enable
+        DeferredBlock.addBlock(() -> {
             m_robotSwerveDrive.resetGyro();
         }, false);
-        DeferredBlock.addBlock(() -> { // Called on every robot enable
+
+        // Called on every robot enable
+        DeferredBlock.addBlock(() -> {
             TimesNegativeOne.update();
+            FieldPositions.update();
         }, true);
 
 
         DriverStation.silenceJoystickConnectionWarning(true);
 
+        // Drive normally
         m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-            // Drive normally
             m_robotSwerveDrive.driveWithInput(
                 getDeadbandedDriverController().getLeft(),
                 getDeadbandedDriverController().getRight(),true);
 
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
+        
         m_robotSwerveDrive.setToSlow();
         
         makeAutoChooser();
@@ -196,6 +202,7 @@ public class RobotContainer {
                 m_robotShooter.io.updateGains();
             }));
 
+            
         // IF the driver is holding the left trigger, intake driving
         new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
             .whileTrue(new RunCommand(
@@ -205,9 +212,6 @@ public class RobotContainer {
                         false
                     );
                 }, m_robotSwerveDrive))
-            // .onFalse(new InstantCommand(() -> , m_robotSwerveDrive));
-
-            
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Extended);
             }))
@@ -222,7 +226,7 @@ public class RobotContainer {
                 () -> {
                 m_robotSwerveDrive.driveFacingPosition(
                     getDeadbandedDriverController().getLeft(),
-                    FieldConstants.BLUE_HUB_POS);
+                    FieldPositions.HUB_POSITION);
                 }, m_robotSwerveDrive)
             )
             .onTrue(new InstantCommand(() -> {

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

@@ -5,15 +5,15 @@ package frc4388.robot.constants;
  */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2026KPopRobotHunters";
+  public static final String MAVEN_NAME = "2026KPopRobotHunters-new";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 38;
+  public static final int GIT_REVISION = 40;
-  public static final String GIT_SHA = "6ce6d0eb0b8faec5af448ec23ffca156303cbed5";
+  public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190";
-  public static final String GIT_DATE = "2026-02-09 19:38:55 MST";
+  public static final String GIT_DATE = "2026-02-10 19:42:47 MST";
-  public static final String GIT_BRANCH = "arm-test";
+  public static final String GIT_BRANCH = "shoot-button";
-  public static final String BUILD_DATE = "2026-02-10 17:12:07 MST";
+  public static final String BUILD_DATE = "2026-02-11 12:41:33 MST";
-  public static final long BUILD_UNIX_TIME = 1770768727439L;
+  public static final long BUILD_UNIX_TIME = 1770838893524L;
-  public static final int DIRTY = 1;
+  public static final int DIRTY = 0;
 
   private BuildConstants(){}
 }

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

@@ -98,13 +98,20 @@ public final class Constants {
 
         public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
 
-        public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
+        // LED color for when the intake is out
-        public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
+        public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED;
-        public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
+        // LED color for when the intake is out, but the driver conditions are bad
-        public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
+        public static final LEDPatterns INTAKE_OUT_BADPHYS = LEDPatterns.RED_STROBE;
 
-        public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
+        // LED color for when the flywheel speed isn't in tolarance
-        public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
+        public static final LEDPatterns BAD_FLYWEEL = LEDPatterns.SOLID_GOLD;
+        // LED color for when the flywheel speed isn't in tolarance, but the driver conditions are bad
+        public static final LEDPatterns BAD_FLYWEEL_BADPHYS = LEDPatterns.GOLD_STROBE;
+
+        // Operator ready to shoot
+        public static final LEDPatterns OPREADY = LEDPatterns.SOLID_WHITE;
+        // Operator ready to shoot, but the driver conditions are bad
+        public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
     }
 
     public static final class OIConstants {

src/main/java/frc4388/robot/subsystems/climber/Climber.java

@@ -1,7 +1,5 @@
 package frc4388.robot.subsystems.climber;
 
-import static edu.wpi.first.units.Units.Rotation;
-
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.Logger;
@@ -9,20 +7,19 @@ import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.utility.status.FaultReporter;
 
 public class Climber extends SubsystemBase {
     ClimberIO io;
     ClimberStateAutoLogged state = new ClimberStateAutoLogged();
 
-    Supplier<Pose2d> m_swervePoseSupplier;
+    // Supplier<Pose2d> m_swervePoseSupplier;
 
     public Climber(
-        ClimberIO io,
+        ClimberIO io
-        Supplier<Pose2d> swervePoseSupplier
+        // Supplier<Pose2d> swervePoseSupplier
     ) {
         this.io = io;
-        this.m_swervePoseSupplier = swervePoseSupplier;
+        // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
     // public enum ClimberMode {
@@ -44,19 +41,6 @@ public class Climber extends SubsystemBase {
     // }
 
 
-    // public enum FieldZone {
-    //     // The robot should aim at the hub
-    //     InShootZone,
-    //     // The robot should aim towards the wall
-    //     AimAtWall,
-        
-    // }
-
-    // // Calculate what should be done based off of the position of the robot
-    // // TODO: Implement field zones
-    // public FieldZone getTarget(Pose2d position) {
-    //     return FieldZone.InShootZone;
-    // }
 
     @Override
     public void periodic() {
@@ -68,9 +52,6 @@ public class Climber extends SubsystemBase {
 
         Logger.processInputs("Climber", state);
 
-        Pose2d pose = m_swervePoseSupplier.get();
-        Angle robotRot = pose.getRotation().getMeasure();
-
         io.updateInputs(state);
 
     }

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

@@ -2,15 +2,21 @@ package frc4388.robot.subsystems.shooter;
 
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import java.text.FieldPosition;
+
 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.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.intake.Intake;
 import frc4388.robot.subsystems.intake.Intake.IntakeMode;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.utility.compute.FieldPositions;
 
 public class Shooter extends SubsystemBase {
     public ShooterIO io;
@@ -57,10 +63,6 @@ public class Shooter extends SubsystemBase {
 
     private ShooterMode mode = ShooterMode.NotReady;
 
-    // public void setMode(ShooterMode mode) {
-    //     this.mode = mode;
-    // }
-
     public void setShooterReady() {
         if(this.mode == ShooterMode.NotReady) {
             this.mode = ShooterMode.Ready;
@@ -86,14 +88,65 @@ public class Shooter extends SubsystemBase {
         io.updateInputs(state);
 
         if(this.mode != ShooterMode.NotReady) {
-            double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2));
+
-            double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond);
+            ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds;
-            double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
+            double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2));
+            double AngSpeed  = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI));
+
+            Pose2d robotPose2d = m_SwerveDrive.getPose2d();
+            
+            double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm();
+
+
+            // TODO: get if the robot is within the angle of the hub
+
+            boolean driverError = 
+                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;
+
+            boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get();
             boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended;
 
+            
+            int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 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: // Bad flywheel, no driver err
+                    m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
+                    break;
+                case 0b011: // Bad flywheel, yes driver err
+                    m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
+                    break;
+                case 0b100: // Bad intake, no driver err
+                    m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
+                    break;
+                case 0b101: // Bad intake, yes driver err
+                    m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
+                    break;
+                case 0b110: // Bad intake and shooter (intake is more important), no driver err
+                    m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT);
+                    break;
+                case 0b111: // Bad intake and shooter (intake is more important), yes driver err
+                    m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS);
+                    break;
+            }
 
+            // We set the shooter mode to ready if there are no errors
+            mode = (
+                bitmask == 0 ?
+                ShooterMode.Ready :
+                ShooterMode.NotReady
+            );
 
-            // TODO: get if the robot is within the correct distance of the hub
         }
 
         switch (mode) {

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

@@ -31,21 +31,21 @@ public class    ShooterConstants {
 
     public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30);
     public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
-    // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
     
     public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10);
     public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
 
+
     // Tolerances
-    public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1);
+    public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0);
-    public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1);
+    public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99);
 
-    public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang  tolerance", 1);
+    public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
 
-    public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1);
+    public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
-    public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1);
+    public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3);
 
-    public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1);
+    public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1); 
 
     
 

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

@@ -0,0 +1,21 @@
+package frc4388.utility.compute;
+
+import edu.wpi.first.math.geometry.Translation2d;
+
+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);
+
+    // We set the default position to one just in case it doesn't update
+    // Setting to null could cause a null pointer, and setting to (0,0) could cause problems
+    // I would rather have the 50/50 chance than a code error
+    public static Translation2d HUB_POSITION = RED_HUB_POSITION;
+
+    public static void update() {
+        if(TimesNegativeOne.isRed) {
+            HUB_POSITION = RED_HUB_POSITION;
+        } else {
+            HUB_POSITION = BLUE_HUB_POSITION;
+        }
+    }
+}