diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1ba7be0..3612c9d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,8 @@ import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.shooter.ShooterGoalPosition; +import frc4388.robot.commands.shooter.ShooterTrenchPosition; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.storage.ManageStorage; @@ -262,13 +264,17 @@ public class RobotContainer { // Goal Shooter Position new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive)) - .whenReleased(new InterruptSubystem(m_robotDrive)); + .whileHeld(new ShooterGoalPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) + .whenReleased(new InterruptSubystem(m_robotShooter)) + .whenReleased(new InterruptSubystem(m_robotShooterHood)) + .whenReleased(new InterruptSubystem(m_robotShooterAim)); // Trench Shooter Position new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive)) - .whenReleased(new InterruptSubystem(m_robotDrive)); + .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) + .whenReleased(new InterruptSubystem(m_robotShooter)) + .whenReleased(new InterruptSubystem(m_robotShooterHood)) + .whenReleased(new InterruptSubystem(m_robotShooterAim)); } /** diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java index acf2366..b750c66 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java @@ -8,13 +8,22 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class ShooterGoalPosition extends CommandBase { + Shooter m_shooter; + ShooterHood m_hood; + ShooterAim m_aim; /** * Creates a new ShooterGoalPosition. */ - public ShooterGoalPosition() { - // Use addRequirements() here to declare subsystem dependencies. + public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) { + m_shooter = shooterSub; + m_hood = hoodSub; + m_aim = aimSub; + addRequirements(m_shooter,m_hood,m_aim); } // Called when the command is initially scheduled. @@ -25,6 +34,9 @@ public class ShooterGoalPosition extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + m_shooter.runDrumShooterVelocityPID(5000); + m_hood.runAngleAdjustPID(3); + m_aim.runshooterRotatePID(-26.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java index f5408f2..ba452b7 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java @@ -8,13 +8,22 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class ShooterTrenchPosition extends CommandBase { + Shooter m_shooter; + ShooterHood m_hood; + ShooterAim m_aim; /** * Creates a new ShooterTrenchPosition. */ - public ShooterTrenchPosition() { - // Use addRequirements() here to declare subsystem dependencies. + public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) { + m_shooter = shooterSub; + m_hood = hoodSub; + m_aim = aimSub; + addRequirements(m_shooter,m_hood,m_aim); } // Called when the command is initially scheduled. @@ -25,6 +34,9 @@ public class ShooterTrenchPosition extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + m_shooter.runDrumShooterVelocityPID(5000); + m_hood.runAngleAdjustPID(3); + m_aim.runshooterRotatePID(-26.5); } // Called once the command ends or is interrupted.