From b182cd74f960448f87fcf0fe46d2e97c66820947 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 9 Mar 2020 19:07:51 -0600 Subject: [PATCH] Debug Storage --- src/main/java/frc4388/robot/Constants.java | 8 ++-- .../java/frc4388/robot/RobotContainer.java | 16 ++++--- .../robot/commands/InterruptSubystem.java | 42 +++++++++++++++++++ .../robot/commands/shooter/PrepChecker.java | 2 +- .../commands/shooter/ShootPrepGroup.java | 2 +- .../robot/commands/storage/StoragePrep.java | 10 ++--- 6 files changed, 61 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/InterruptSubystem.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cd211c6..4782ff4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -171,10 +171,10 @@ public final class Constants { public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt /* Ball Indexes */ - public static final int BEAM_SENSOR_SHOOTER = 1; - public static final int BEAM_SENSOR_USELESS = 2; - public static final int BEAM_SENSOR_STORAGE = 3; - public static final int BEAM_SENSOR_INTAKE = 4; + public static final int BEAM_SENSOR_SHOOTER = 11; + public static final int BEAM_SENSOR_USELESS = 12; + public static final int BEAM_SENSOR_STORAGE = 13; + public static final int BEAM_SENSOR_INTAKE = 14; /* PID Gains */ public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 17f28d0..fd9a56c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.InterruptSubystem; import frc4388.robot.commands.auto.AutoPath1FromCenter; import frc4388.robot.commands.auto.Wait; import frc4388.robot.commands.climber.DisengageRachet; @@ -43,6 +44,7 @@ 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.StoragePrep; import frc4388.robot.commands.storage.ManageStorage.StorageMode; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; @@ -171,19 +173,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 InterruptSubystem(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 InterruptSubystem(m_robotStorage)); // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) @@ -203,6 +205,8 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new TrackTarget(m_robotShooterAim)) .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) + //.whenPressed(new StoragePrep(m_robotStorage)) + //.whenReleased(new InterruptSubystem(m_robotStorage)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000))); //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) diff --git a/src/main/java/frc4388/robot/commands/InterruptSubystem.java b/src/main/java/frc4388/robot/commands/InterruptSubystem.java new file mode 100644 index 0000000..5eae65f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/InterruptSubystem.java @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class InterruptSubystem extends CommandBase { + /** + * Creates a new InterruptSubystem. + */ + public InterruptSubystem(SubsystemBase subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java index c0ef7a6..f76fd3e 100644 --- a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java +++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java @@ -68,7 +68,7 @@ public class PrepChecker extends CommandBase { @Override public boolean isFinished() { if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) { - return true; + //return true; } return false; } diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index b328f71..d669dd1 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -27,7 +27,7 @@ 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 StoragePrep(m_storage), new TrackTarget(m_shooterAim), new ShooterVelocityControlPID(m_shooter), new HoodPositionPID(m_shooterHood) diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java index d42c9b9..9fcee06 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java @@ -32,10 +32,12 @@ public class StoragePrep extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (!m_storage.m_isStorageReadyToFire) { + if ((m_storage.getBeamUseless() && m_storage.getBeamShooter()) || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) { m_storage.runStorage(StorageConstants.STORAGE_SPEED); + m_storage.m_isStorageReadyToFire = false; } else { m_storage.runStorage(0); + m_storage.m_isStorageReadyToFire = true; } } @@ -47,12 +49,6 @@ public class StoragePrep extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeamShooter() || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) { - m_storage.m_isStorageReadyToFire = true; - } else { - m_storage.m_isStorageReadyToFire = false; - } - return false; } }