Improve shooter

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

@@ -8,6 +8,7 @@ import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants;
 import frc4388.robot.subsystems.LED;
@@ -50,31 +51,24 @@ 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 setShooterShooting() {
-        this.mode = ShooterMode.Ready;
+        this.mode = ShooterMode.Shooting;
     }
 
-    public void setShooterReadyFeeder() {
+    public void setShooterFeeding() {
-        this.mode = ShooterMode.ReadyFeeder;
+        this.mode = ShooterMode.Feeding;
     }
     
-    public void setShooterNotReady() {
+    public void setShooterIdle() {
-        this.mode = ShooterMode.NotReady;
+        this.mode = ShooterMode.Idle;
     }
 
 
@@ -86,6 +80,7 @@ public class Shooter extends SubsystemBase {
         shooterButtonReady = false;
     }
 
+
     @AutoLogOutput
     public ShooterMode getMode() {
         return mode;
@@ -111,114 +106,104 @@ public class Shooter extends SubsystemBase {
         Logger.recordOutput("Hub Dist", distanceToHub);
 
 
-        if(this.mode != ShooterMode.NotReady) {
-            // TODO: get if the robot is within the angle of the hub
 
-            boolean driverError = 
+        boolean driverError = 
-                // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
+            // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
-                // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
+            // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
-                distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | 
+            distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | 
-                distanceToHub >= ShooterConstants.ROBOT_MAX_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;
-
-            boolean feedMdoe = this.mode == ShooterMode.ReadyFeeder |
-                this.mode == ShooterMode.ShootingFeeder;
-
-            int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (
-                (feedMdoe) ? 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:
-                case 0b101:
-                    m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
-                    break;
-            }
-
-        //     // We set the shooter mode to ready if there are no errors
-            
-            if (!feedMdoe) {
-                mode = (
-                    bitmask == 0 ?
-                    ShooterMode.Shooting :
-                    ShooterMode.Ready
-                );
-            } else {
-
-                if(bitmask == 0b100 |
-                bitmask == 0b101) {
-                    mode = ShooterMode.ShootingFeeder;
-
-                } else {
-                    mode = ShooterMode.ReadyFeeder;
-                }
-                
-            }
-
-        } 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, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get()));
-                io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
                 break;
-
+            case Idle:
-            case NotReady:
                 io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
                 io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
                 break;
-        }
+            }
 
     }
 }