Merge pull request #4 from Team4388/Improve-shooter

Improve shooter

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

@@ -99,7 +99,7 @@ public class RobotContainer {
     );
     
     private Command RobotShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotShooter.setShooterReady()),
+        new InstantCommand(() -> m_robotShooter.spinUpShooting()),
         new RunCommand(
                 () -> {
                 m_robotSwerveDrive.driveFacingPosition(
@@ -110,9 +110,9 @@ public class RobotContainer {
                 }, m_robotSwerveDrive),
         new InstantCommand(()->m_robotIntake.setMode(IntakeMode.Idle)),
         new WaitCommand(5),
-        new InstantCommand(()->m_robotShooter.setShooterShoot()),
+        new InstantCommand(()->m_robotShooter.allowShooting()),
         new WaitCommand(10),
-        new InstantCommand(()->m_robotShooter.setShooterNOTShoot())
+        new InstantCommand(()->m_robotShooter.denyShooting())
     );
 
     // private Command RobotShoot = new SequentialCommandGroup(
@@ -256,18 +256,18 @@ 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();
             }));
 
         
@@ -275,9 +275,9 @@ public class RobotContainer {
 
         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/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,42 +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 setShooterReadyFeeder() {
+    public void spinUpFeeding() {
-        this.mode = ShooterMode.ReadyFeeder;
+        this.mode = ShooterMode.Feeding;
     }
     
-    public void setShooterNotReady() {
+    public void spinUpIdle() {
-        this.mode = ShooterMode.NotReady;
+        this.mode = ShooterMode.Idle;
     }
 
 
-    public void setShooterShoot() {
+    public void allowShooting() {
         shooterButtonReady = true;
     }
 
-    public void setShooterNOTShoot() {
+    public void denyShooting() {
         shooterButtonReady = false;
     }
 
+
     @AutoLogOutput
     public ShooterMode getMode() {
         return mode;
@@ -100,123 +98,131 @@ 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);
 
-
-        if(this.mode != ShooterMode.NotReady) {
-            // 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;
         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);
-
-        }
-        
-        
-
 
         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) +
+                    (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 = (
+                    (shooterButtonReady ? 1 : 0) +
+                    (badShooterVelocity ? 2 : 0)
+                );
+
+                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());
-                } else {
+                        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.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;