diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java deleted file mode 100644 index f76fd3e..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java +++ /dev/null @@ -1,75 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.shooter; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; - -public class PrepChecker extends CommandBase { - Shooter m_shooter; - ShooterAim m_shooterAim; - ShooterHood m_shooterHood; - Storage m_storage; - - boolean m_isDrumReady = false; - boolean m_isAimReady = false; - boolean m_isHoodReady = false; - boolean m_isStorageReady = false; - - /** - * Creates a new PrepChecker. - * @param shooter used to read all shooter subsystems. Not used as a requirement so don't expect it to interrupt other commands. - * @param storage reads storage in a similar way to shooter. Not used as a requirement. - */ - public PrepChecker(Shooter shooter, Storage storage) { - m_shooter = shooter; - m_shooterAim = m_shooter.m_shooterAimSubsystem; - m_shooterHood = m_shooter.m_shooterHoodSubsystem; - m_storage = storage; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_isDrumReady = false; - m_isAimReady = false; - m_isHoodReady = false; - m_isStorageReady = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_isDrumReady = m_shooter.m_isDrumReady; SmartDashboard.putBoolean("ShooterVelocityPID Finished", m_isDrumReady); - m_isAimReady = m_shooterAim.m_isAimReady; SmartDashboard.putBoolean("TrackTarget Finished", m_isAimReady); - m_isHoodReady = m_shooterHood.m_isHoodReady; SmartDashboard.putBoolean("HoodPosition Finished", m_isHoodReady); - m_isStorageReady = m_storage.m_isStorageReadyToFire; SmartDashboard.putBoolean("StoragePrepAim Finished", m_isStorageReady); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_shooter.m_isDrumReady = false; - m_shooterAim.m_isAimReady = false; - m_shooterHood.m_isHoodReady = false; - m_storage.m_isStorageReadyToFire = false; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (m_isDrumReady && m_isAimReady && m_isHoodReady && m_isStorageReady) { - //return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java deleted file mode 100644 index 40cbaa4..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFireGroup.java +++ /dev/null @@ -1,37 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.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; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -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 ParallelDeadlineGroup { - /** - * Fires the shooter - * @param m_shooter The Shooter subsytem - * @param m_shooterAim The ShooterAim subsystem - * @param m_storage The Storage subsytem - */ - public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - super( - new StorageFire(m_storage), - new TrackTarget(m_shooterAim), - new ShooterVelocityControlPID(m_shooter), - new HoodPositionPID(m_shooterHood) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java deleted file mode 100644 index 8863636..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShootFullGroup.java +++ /dev/null @@ -1,32 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.shooter; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -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 ShootFullGroup extends SequentialCommandGroup { - /** - * Preps and Fires the Shooter - * @param m_shooter The Shooter subsytem - * @param m_shooterAim The ShooterAim subsystem - * @param m_storage The Storage subsytem - */ - public ShootFullGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - addCommands( - new ShootPrepGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage), - new ShootFireGroup(m_shooter, m_shooterAim, m_shooterHood, m_storage) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index d669dd1..df92eb4 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -26,8 +26,6 @@ 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) diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFire.java b/src/main/java/frc4388/robot/commands/storage/StorageFire.java deleted file mode 100644 index 261975e..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StorageFire.java +++ /dev/null @@ -1,48 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StorageFire extends CommandBase { - Storage m_storage; - /** - * Runs the Storage until shoot beam is empty, then ends - * @param storageSub The Storage subsytem - */ - public StorageFire(Storage storageSub) { - m_storage = storageSub; - } - - // 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() { - m_storage.runStorage(StorageConstants.STORAGE_SPEED); - } - - // 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() { - if (m_storage.getBeamShooter()) { - return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java b/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java deleted file mode 100644 index 6026452..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StorageFirePID.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StorageFirePID extends CommandBase { - Storage m_storage; - double m_intakeStartPos; - - /** - * Runs the Storage until shoot beam is empty, then ends - * @param storageSub The Storage subsytem - */ - public StorageFirePID(Storage storageSub) { - m_storage = storageSub; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_intakeStartPos = m_storage.getEncoderPosInches(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_storage.runStoragePositionPID(m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL); - } - - // 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() { - double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches(); - if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) { - return true; - } - return false; - } -}