Merge pull request #31 from StuyPulse/8/18

8/18
StuyPulse · Aug 22, 2024 · 12ea26e · 12ea26e
2 parents 3d9e773 + 1d02dd0
commit 12ea26e
Show file tree

Hide file tree

Showing 13 changed files with 93 additions and 65 deletions.
diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java
@@ -3,6 +3,7 @@
 import com.stuypulse.robot.commands.BuzzController;
 import com.stuypulse.robot.commands.intake.IntakeShoot;
 import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
+import com.stuypulse.robot.commands.shooter.ShooterManualIntake;
 import com.stuypulse.robot.commands.vision.VisionReloadWhiteList;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Settings.RobotType;
@@ -51,9 +52,20 @@ public void robotPeriodic() {
         }
 
         if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
-            && Arm.getInstance().atValidFeedAngle()
+            && Arm.getInstance().atIntakeShouldShootAngle()
         ) {
-            CommandScheduler.getInstance().schedule(new IntakeShoot().until(() -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED));
+            CommandScheduler.getInstance().schedule(new IntakeShoot()
+                                                    .until(
+                                                        () -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED
+                                                            || !Arm.getInstance().atIntakeShouldShootAngle()
+                                                    ));
+        }
+
+        if (Arm.getInstance().getState() == Arm.State.AMP 
+            && !Shooter.getInstance().hasNote() 
+            && Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING
+        ) {
+            CommandScheduler.getInstance().schedule(new ShooterManualIntake().until(() -> Arm.getInstance().getState() != Arm.State.AMP));
         }
 
         CommandScheduler.getInstance().run();

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -162,7 +162,7 @@ private void configureDriverBindings() {
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker()))
                         .andThen(new ShooterFeederShoot()
-                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
                             )
                     )
                     .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
@@ -176,14 +176,14 @@ private void configureDriverBindings() {
                 () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED));
 
         // lob ferry align and shoot
-        driver.getDPadRight()
+        driver.getLeftStickButton()
             .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver)
                     .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry()))
                         .andThen(new ShooterFeederShoot()
-                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
                             )
                     )
                     .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)
@@ -198,14 +198,14 @@ private void configureDriverBindings() {
 
 
         // low ferry align and shoot
-        driver.getDPadDown()
+        driver.getRightStickButton()
             .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver)
                     .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds()))
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry()))
                         .andThen(new ShooterFeederShoot()
-                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
                             )
                     )
                     .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)
@@ -227,7 +227,7 @@ private void configureDriverBindings() {
                         .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET)
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new ShooterFeederShoot()
-                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
                             )
                         )
             .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL))
@@ -244,7 +244,7 @@ private void configureDriverBindings() {
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry()))
                         .andThen(new ShooterFeederShoot()
-                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
                             )
                     )
                     .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)
@@ -265,7 +265,7 @@ private void configureDriverBindings() {
                                 .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)))
                         .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry()))
                         .andThen(new ShooterFeederShoot()
-                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle()))
+                            .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle()))
                         )
                     )
                     .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)
@@ -281,8 +281,6 @@ private void configureDriverBindings() {
         // climbing
         driver.getRightButton().onTrue(new ArmToPreClimb());
         driver.getBottomButton().onTrue(new ArmToClimbing());
-
-        driver.getLeftMenuButton().whileTrue(new ShooterManualIntake());
     }
 
     private void configureOperatorBindings() {

diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java
@@ -29,7 +29,7 @@ public ShooterAcquireFromIntake() {
     public void initialize() {
         stopWatch.reset();
         intake.setState(Intake.State.FEEDING);
-        shooter.feederIntake();
+        shooter.setFeederState(Shooter.FeederState.INTAKING);
     }
 
     @Override
@@ -57,7 +57,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        shooter.feederStop();
+        shooter.setFeederState(Shooter.FeederState.STOP);
         intake.setState(Intake.State.STOP);
     }
 

diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java
@@ -15,7 +15,7 @@ public ShooterFeederAcquire() {
 
     @Override
     public void initialize() {
-        shooter.feederIntake();
+        shooter.setFeederState(Shooter.FeederState.INTAKING);
     }
 
 }
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java
@@ -15,6 +15,6 @@ public ShooterFeederDeacquire() {
 
     @Override
     public void initialize() {
-        shooter.feederDeacquire();
+        shooter.setFeederState(Shooter.FeederState.DEACQUIRING);
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java
@@ -15,6 +15,6 @@ public ShooterFeederShoot() {
 
     @Override
     public void initialize() {
-        shooter.feederShoot();
+        shooter.setFeederState(Shooter.FeederState.SHOOTING);
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java
@@ -15,7 +15,7 @@ public ShooterFeederStop() {
 
     @Override
     public void initialize() {
-        shooter.feederStop();
+        shooter.setFeederState(Shooter.FeederState.STOP);
     }
 
 }
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java
@@ -15,11 +15,16 @@ public ShooterManualIntake() {
 
     @Override
     public void initialize() {
-        shooter.feederIntake();
+        shooter.setFeederState(Shooter.FeederState.INTAKING);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return shooter.hasNote();
     }
 
     @Override
     public void end(boolean interrupted) {
-        shooter.feederStop();
+        shooter.setFeederState(Shooter.FeederState.STOP);
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -58,8 +58,8 @@ public static RobotType fromString(String serialNum) {
     public interface Arm {
         double LENGTH = Units.inchesToMeters(16.5);
 
-        SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 1000);
-        SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 800);
+        SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 900);
+        SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 700);
 
         SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85);
         SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25);
@@ -71,8 +71,10 @@ public interface Arm {
         SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50);
         SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80);
         SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7);
+
         SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0);
-        SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 20);
+        SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4);
+
         SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -33);
 
         SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02);
@@ -100,11 +102,12 @@ public interface Encoder {
     }
 
     public interface Intake {
-        double INTAKE_ACQUIRE_SPEED = 1.0;
+        double INTAKE_ACQUIRE_SPEED = 0.72;
         double INTAKE_DEACQUIRE_SPEED = 1.0;
 
-        double INTAKE_FEED_SPEED = 0.48; 
+        double INTAKE_FEED_SPEED = 0.4; 
 
+        double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 25;
         double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second
 
         double INTAKE_SHOOT_SPEED = 0.9;
@@ -113,14 +116,14 @@ public interface Intake {
         double FUNNEL_ACQUIRE = 1.0;
         double FUNNEL_DEACQUIRE = 1.0;
 
-        double IR_DEBOUNCE = .005;
+        double IR_DEBOUNCE = 0.0;
 
-        double HANDOFF_TIMEOUT = 1.5;
+        double HANDOFF_TIMEOUT = 1.0;
         double MINIMUM_DEACQUIRE_TIME_WHEN_STUCK = 0.5;
     }
 
     public interface Shooter {
-        double FEEDER_INTAKE_SPEED = 0.19;
+        double FEEDER_INTAKE_SPEED = 0.18;
         double FEEDER_DEAQUIRE_SPEED = 0.5;
         double FEEDER_SHOOT_SPEED = 1.0;
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java
@@ -45,6 +45,7 @@ public State getState() {
     public abstract boolean atTarget();
 
     public abstract boolean atValidFeedAngle();
+    public abstract boolean atIntakeShouldShootAngle();
 
     public abstract double getVelocity();
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java
@@ -77,7 +77,12 @@ public boolean atTarget() {
     @Override
     public boolean atValidFeedAngle() {
         return getDegrees() > Settings.Arm.FEED_ANGLE.get() - Settings.Arm.MAX_ANGLE_ERROR.get()
-                    || getDegrees() < Settings.Arm.MAX_ACCEPTABLE_FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get();
+                    && getDegrees() < Settings.Arm.MAX_ACCEPTABLE_FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get();
+    }
+
+    @Override
+    public boolean atIntakeShouldShootAngle() {
+        return getDegrees() < Settings.Intake.MAX_ARM_ANGLE_FOR_INTAKE_SHOOT;
     }
 
     private double getTargetDegrees() {
@@ -173,7 +178,7 @@ public void periodic() {
         }
         else {
             controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees());
-            if (Shooter.getInstance().isShooting()) {
+            if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING) {
                 setVoltage(controller.getOutput() + 0.31);
             }
             else {

diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java
@@ -17,11 +17,21 @@ public abstract class Shooter extends SubsystemBase {
     public static Shooter getInstance() {
         return instance;
     }
+
+    public enum FeederState {
+        INTAKING,
+        DEACQUIRING,
+        SHOOTING,
+        STOP
+    }
+
+    private FeederState feederState;
 
     private final SmartNumber leftTargetRPM;
     private final SmartNumber rightTargetRPM;
 
     public Shooter() {
+        feederState = FeederState.STOP;
         leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getLeftRPM() : 0);
         rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getRightRPM() : 0);
     }
@@ -40,21 +50,22 @@ public double getRightTargetRPM() {
     }
 
     public void stop() {
-        feederStop();
+        setFeederState(FeederState.STOP);
         leftTargetRPM.set(0);
         rightTargetRPM.set(0);
     }
 
-    public abstract boolean atTargetSpeeds();
+    public void setFeederState(FeederState feederState) {
+        this.feederState = feederState;
+    }
 
-    public abstract ShooterSpeeds getFerrySpeeds();
+    public FeederState getFeederState() {
+        return feederState;
+    }
 
-    public abstract void feederIntake();
-    public abstract void feederDeacquire();
-    public abstract void feederShoot();
-    public abstract void feederStop();
+    public abstract boolean atTargetSpeeds();
 
-    public abstract boolean isShooting();
+    public abstract ShooterSpeeds getFerrySpeeds();
 
     public abstract boolean hasNote();
 }
diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java
@@ -106,33 +106,24 @@ private void setRightShooterRPM(double rpm) {
         rightController.setReference(rpm, ControlType.kVelocity);
     }
 
-    @Override
-    public void feederIntake() {
-        feederMotor.set(+Settings.Shooter.FEEDER_INTAKE_SPEED);
-        isShooting = false;
-    }
-
-    @Override
-    public void feederDeacquire() {
-        feederMotor.set(-Settings.Shooter.FEEDER_DEAQUIRE_SPEED);
-        isShooting = false;
-    }
-
-    @Override
-    public void feederShoot() {
-        feederMotor.set(Settings.Shooter.FEEDER_SHOOT_SPEED);
-        isShooting = true;
-    }
-
-    @Override
-    public void feederStop() {
-        feederMotor.set(0);
-        isShooting = false;
-    }
-
-    @Override
-    public boolean isShooting() {
-        return isShooting;
+    private void setFeederBasedOnState() {
+        switch (getFeederState()) {
+            case INTAKING:
+                feederMotor.set(+Settings.Shooter.FEEDER_INTAKE_SPEED);
+                break;
+            case DEACQUIRING:
+                feederMotor.set(-Settings.Shooter.FEEDER_DEAQUIRE_SPEED);
+                break;
+            case SHOOTING:
+                feederMotor.set(Settings.Shooter.FEEDER_SHOOT_SPEED);
+                break;
+            case STOP:
+                feederMotor.set(0);
+                break;
+            default:
+                feederMotor.set(0);
+                break;
+        }
     }
 
     @Override
@@ -176,6 +167,8 @@ public void periodic () {
             setRightShooterRPM(getRightTargetRPM());
         }
 
+        setFeederBasedOnState();
+
         SmartDashboard.putNumber("Shooter/Feeder Speed", feederMotor.get());
 
         SmartDashboard.putNumber("Shooter/Left Voltage", leftMotor.getBusVoltage());