diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 1efc6f4..8a3fbe8 100644 --- 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 index b0e7e01..fda91b3 100644 --- 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 index 5346069..95c297e 100644 --- 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 index ca4cca0..72656e4 100644 --- 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); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java index d82326e..657e782 100644 --- 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); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java index 674c24d..30ef6f2 100644 --- 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); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java index 6201923..788d2dd 100644 --- 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); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java index 4e0d017..d623417 100644 --- 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 index e915fc4..a21803a 100644 --- 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 index e6fc3ea..a3547b7 100644 --- 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 index 9aaf957..2268399 100644 --- 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 index fa50c6b..6f5fad3 100644 --- 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 index 62ecdeb..7cecf96 100644 --- 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());