diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e7130ca..017a6b9 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -1,8 +1,10 @@ package com.stuypulse.robot; 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.vision.VisionReloadWhiteList; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.RobotType; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; @@ -47,6 +49,12 @@ public void robotPeriodic() { CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake() .andThen(new BuzzController(robot.driver))); } + + if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + && Arm.getInstance().atValidFeedAngle() + ) { + CommandScheduler.getInstance().schedule(new IntakeShoot()); + } CommandScheduler.getInstance().run(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 58132af..b0e7e01 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,7 +13,10 @@ import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; +import com.stuypulse.robot.commands.intake.IntakeFeed; +import com.stuypulse.robot.commands.intake.IntakeShoot; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.leds.LEDDefaultMode; import com.stuypulse.robot.commands.leds.LEDReset; @@ -22,6 +25,7 @@ import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.shooter.ShooterStop; @@ -106,6 +110,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); + intake.setDefaultCommand(new IntakeStop()); leds.setDefaultCommand(new LEDDefaultMode()); } @@ -124,7 +129,7 @@ private void configureDriverBindings() { // intake field relative driver.getRightTriggerButton() .onTrue(new ArmToFeed()) - .whileTrue(new SwerveDriveDriveToNote(driver)) + // .whileTrue(new SwerveDriveDriveToNote(driver)) .whileTrue(new IntakeAcquire() .deadlineWith(new LEDSet(LEDInstructions.INTAKING)) .andThen(new BuzzController(driver)) @@ -141,9 +146,8 @@ private void configureDriverBindings() { // deacquire driver.getDPadLeft() - .onTrue(new IntakeDeacquire()) - .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)) - .onFalse(new IntakeStop()); + .whileTrue(new IntakeDeacquire()) + .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)); // speaker align and score // score amp @@ -157,7 +161,9 @@ 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 WaitUntilCommand(() -> swerve.isAlignedToSpeaker())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle())) + ) ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN) .until(() -> swerve.isAlignedToSpeaker()) @@ -169,15 +175,16 @@ private void configureDriverBindings() { new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // ferry align and shoot - // move to back of controller + // lob ferry align and shoot driver.getDPadRight() .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()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle())) + ) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN) .until(() -> swerve.isAlignedToLobFerry()) @@ -189,13 +196,17 @@ private void configureDriverBindings() { new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + + // low ferry align and shoot driver.getDPadDown() .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()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle())) + ) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN) .until(() -> swerve.isAlignedToLowFerry()) @@ -207,36 +218,34 @@ private void configureDriverBindings() { new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // arm to amp and alignment - driver.getLeftBumper() - .onTrue(new ArmToAmp()) - .onTrue(new SwerveDriveDriveAlignedAmp(driver) - .onlyWhile(() -> - Math.abs(driver.getRightX()) < Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble() && - Arm.getInstance().getState() == Arm.State.AMP) - .deadlineWith(new LEDSet(LEDInstructions.AMP_WITH_ALIGN))); + // arm to amp + driver.getLeftBumper().onTrue(new ArmToAmp()); // manual speaker at subwoofer - // rebind to a button on the back later driver.getRightMenuButton() .whileTrue(new ArmToSubwooferShot().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .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())) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle())) + ) + ) .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL)) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // manual ferry + // manual lob ferry driver.getTopButton() .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(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.isAlignedToManualLobFerry())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle())) + ) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL) .until(() -> swerve.isAlignedToManualLobFerry()) @@ -248,13 +257,16 @@ private void configureDriverBindings() { new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + // manual low ferry driver.getLeftButton() .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(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.isAlignedToManualLowFerry())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atValidFeedAngle())) + ) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL) .until(() -> swerve.isAlignedToManualLowFerry()) @@ -269,6 +281,8 @@ 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/intake/IntakeDeacquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java index 335cfbc..4c8ecdb 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java @@ -2,9 +2,9 @@ import com.stuypulse.robot.subsystems.intake.Intake; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class IntakeDeacquire extends InstantCommand { +public class IntakeDeacquire extends Command { private final Intake intake; diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java new file mode 100644 index 0000000..a0afb12 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class IntakeShoot extends SequentialCommandGroup { + + public IntakeShoot() { + addCommands( + new InstantCommand(() -> Intake.getInstance().setState(Intake.State.SHOOTING), Intake.getInstance()), + new WaitCommand(Settings.Intake.INTAKE_SHOOT_TIME), + new IntakeStop() + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java new file mode 100644 index 0000000..4e0d017 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java @@ -0,0 +1,25 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterManualIntake extends Command{ + + private final Shooter shooter; + + public ShooterManualIntake() { + shooter = Shooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.feederIntake(); + } + + @Override + public void end(boolean interrupted) { + shooter.feederStop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index faef31a..e915fc4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -58,20 +58,21 @@ 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 : 350); - SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 350); + 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_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85); SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25); SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); - SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 50); + SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 40); SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50); 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() + 17); + 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 SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -33); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -99,10 +100,15 @@ public interface Encoder { } public interface Intake { - double INTAKE_ACQUIRE_SPEED = 0.65; + double INTAKE_ACQUIRE_SPEED = 1.0; double INTAKE_DEACQUIRE_SPEED = 1.0; - double INTAKE_FEED_SPEED = 0.65; + double INTAKE_FEED_SPEED = 0.48; + + double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second + + double INTAKE_SHOOT_SPEED = 0.9; + double INTAKE_SHOOT_TIME = 0.75; double FUNNEL_ACQUIRE = 1.0; double FUNNEL_DEACQUIRE = 1.0; @@ -114,7 +120,7 @@ public interface Intake { } public interface Shooter { - double FEEDER_INTAKE_SPEED = 0.23; + double FEEDER_INTAKE_SPEED = 0.19; double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; @@ -129,8 +135,8 @@ public interface Shooter { ); // Different falling debounce is used to detect note shooting; - SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.01); - SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.005); + SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); //0.01 + SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); //0.005 // left runs faster than right public interface LEFT { @@ -404,8 +410,8 @@ public interface Vision { } public interface Buzz { - double BUZZ_DURATION = 0.2; - double BUZZ_INTENSITY = 1; + double BUZZ_DURATION = 1.0; + double BUZZ_INTENSITY = 1.0; } public interface Auton { 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 adbfd16..e6fc3ea 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -44,6 +44,10 @@ public State getState() { public abstract boolean atTarget(); + public abstract boolean atValidFeedAngle(); + + public abstract double getVelocity(); + @Override public void periodic() { SmartDashboard.putString("Arm/State", state.toString()); 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 415cd6d..9aaf957 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -69,11 +69,17 @@ protected ArmImpl() { @Override public boolean atTarget() { if (state == State.FEED) { - return getDegrees() < Settings.Arm.FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get(); + return atValidFeedAngle(); } return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get(); } + @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(); + } + private double getTargetDegrees() { switch (state) { case AMP: @@ -140,6 +146,11 @@ private double getDegrees() { return 360 * armEncoder.getPosition(); } + @Override + public double getVelocity() { + return 360 / 60 * armEncoder.getVelocity(); + } + private void setVoltage(double voltage) { leftMotor.setVoltage(voltage); rightMotor.setVoltage(voltage); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 9f6ba13..43b7ddd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.subsystems.intake; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -19,6 +20,7 @@ public enum State { DEACQUIRING, ACQUIRING, FEEDING, + SHOOTING, STOP } @@ -37,4 +39,9 @@ public State getState() { } public abstract boolean hasNote(); + + @Override + public void periodic() { + SmartDashboard.putString("Intake/State", state.name()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 021e797..01948ed 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -44,6 +44,12 @@ private void acquire() { funnelMotorRight.set(+Settings.Intake.FUNNEL_ACQUIRE); } + private void shoot() { + intakeMotor.set(Settings.Intake.INTAKE_SHOOT_SPEED); + funnelMotorLeft.stopMotor(); + funnelMotorRight.stopMotor(); + } + private void deacquire() { intakeMotor.set(-Settings.Intake.INTAKE_DEACQUIRE_SPEED); funnelMotorLeft.set(-Settings.Intake.FUNNEL_DEACQUIRE); @@ -81,6 +87,9 @@ public void periodic() { case FEEDING: feed(); break; + case SHOOTING: + shoot(); + break; case STOP: stop(); break;