diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cbf0e96..cd211c6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -163,7 +163,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; - public static final double STORAGE_SPEED = 0.5; + public static final double STORAGE_SPEED = 1.0; public static final double STORAGE_TIMEOUT = 3000; /* Storage Characteristics */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7588412..b7a17ce 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,6 +42,8 @@ import frc4388.robot.commands.shooter.ShootFullGroup; import frc4388.robot.commands.shooter.ShootPrepGroup; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; +import frc4388.robot.commands.storage.ManageStorage; +import frc4388.robot.commands.storage.ManageStorage.StorageMode; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; @@ -169,17 +171,19 @@ public class RobotContainer { /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - //.whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); + //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + //.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) + //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false); + //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + //.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) + //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -199,7 +203,6 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooterAim)) .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) - //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) @@ -226,7 +229,8 @@ public class RobotContainer { //Run drum new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new ShootFireGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java index ce18d4b..40cbaa4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java @@ -7,6 +7,7 @@ package frc4388.robot.commands.shooter; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.robot.commands.storage.StorageFire; @@ -18,7 +19,7 @@ import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShootFireGroup extends ParallelRaceGroup { +public class ShootFireGroup extends ParallelDeadlineGroup { /** * Fires the shooter * @param m_shooter The Shooter subsytem @@ -26,11 +27,11 @@ public class ShootFireGroup extends ParallelRaceGroup { * @param m_storage The Storage subsytem */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()), m_shooter), - new RunCommand(() -> m_shooterHood.runAngleAdjustPID(m_shooterHood.addFireAngle()), m_shooterHood), + super( + new StorageFire(m_storage), new TrackTarget(m_shooterAim), - new StorageFire(m_storage) + new ShooterVelocityControlPID(m_shooter), + new HoodPositionPID(m_shooterHood) ); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index d919d32..b328f71 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -27,10 +27,10 @@ public class ShootPrepGroup extends ParallelDeadlineGroup { public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { super( new PrepChecker(m_shooter, m_storage), + new StoragePrep(m_storage), new TrackTarget(m_shooterAim), new ShooterVelocityControlPID(m_shooter), - new HoodPositionPID(m_shooterHood), - new StoragePrep(m_storage) + new HoodPositionPID(m_shooterHood) ); } } diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java index 38a7670..4e019c7 100644 --- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java @@ -27,7 +27,7 @@ public class ManageStorage extends CommandBase { /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ boolean m_isStorageEmpty = true; - enum StorageMode{IDLE, INTAKE, RESET}; + public enum StorageMode{IDLE, INTAKE, RESET}; StorageMode m_storageMode = StorageMode.IDLE; /** diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFire.java b/src/main/java/frc4388/robot/commands/storage/StorageFire.java index 61dda95..261975e 100644 --- a/src/main/java/frc4388/robot/commands/storage/StorageFire.java +++ b/src/main/java/frc4388/robot/commands/storage/StorageFire.java @@ -14,7 +14,7 @@ import frc4388.robot.subsystems.Storage; public class StorageFire extends CommandBase { Storage m_storage; /** - * Runs the Storage at a speed + * Runs the Storage until shoot beam is empty, then ends * @param storageSub The Storage subsytem */ public StorageFire(Storage storageSub) { @@ -40,6 +40,9 @@ public class StorageFire extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + if (m_storage.getBeamShooter()) { + return true; + } return false; } } diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java index 0f39d25..cee4b16 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java @@ -32,7 +32,11 @@ public class StoragePrep extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_storage.runStorage(StorageConstants.STORAGE_SPEED); + if (!m_storage.m_isStorageReadyToFire) { + m_storage.runStorage(StorageConstants.STORAGE_SPEED); + } else { + m_storage.runStorage(0); + } } // Called once the command ends or is interrupted. @@ -45,10 +49,10 @@ public class StoragePrep extends CommandBase { public boolean isFinished() { if (!m_storage.getBeamShooter() || (startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) { m_storage.m_isStorageReadyToFire = true; - return true; } else { m_storage.m_isStorageReadyToFire = false; - return false; } + + return false; } }