diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9821f80..23a2276 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,7 +30,7 @@ public class RobotContainer { private final Stager subStager = new Stager(); private final Drive com_Drive = new Drive(subDrivetrain, m_driverController.axis_LeftY, m_driverController.axis_RightX); - private final IntakeGround com_IntakeGround = new IntakeGround(subIntake, subHopper); + private final IntakeGround com_IntakeGround = new IntakeGround(subIntake, subStager); private final PrepShooter com_PrepShooter = new PrepShooter(subShooter); private final HasGP com_StageGP = new HasGP(subStager); private final Shoot com_Shoot = new Shoot(subStager, subShooter); @@ -42,7 +42,7 @@ public RobotContainer() { private void configureBindings() { m_driverController.btn_B.whileTrue(com_IntakeGround); - m_driverController.btn_LeftBumper.whileTrue(new intakeHopper(subHopper)); + m_driverController.btn_LeftBumper.whileTrue(new intakeHopper(subHopper, subStager)); m_driverController.btn_X.whileTrue(com_PrepShooter); m_driverController.btn_A.whileTrue(com_StageGP); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 83fd62e..e940c63 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -15,7 +15,6 @@ public static class mapControllers { public static class mapHopper { public static final int ORIENTATION_MOTOR = 30; public static final int IS_GP_DETECTED_DIO = 1; - public static final int IS_HOPPER_FULL_DIO = 2; } public static class mapDriveTrain { diff --git a/src/main/java/frc/robot/commands/IntakeGround.java b/src/main/java/frc/robot/commands/IntakeGround.java index fd659fe..5dc9b70 100644 --- a/src/main/java/frc/robot/commands/IntakeGround.java +++ b/src/main/java/frc/robot/commands/IntakeGround.java @@ -5,18 +5,18 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Hopper; +import frc.robot.subsystems.Stager; import frc.robot.subsystems.Intake; public class IntakeGround extends Command { /** Creates a new IntakeGround. */ Intake globalIntake; - Hopper globalHopper; + Stager globalStager; - public IntakeGround(Intake passedIntake, Hopper passedHopper) { + public IntakeGround(Intake passedIntake, Stager passedStager) { // Use addRequirements() here to declare subsystem dependencies. globalIntake = passedIntake; - globalHopper = passedHopper; + globalStager = passedStager; } // Called when the command is initially scheduled. @@ -28,8 +28,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // if hopper is full - if (globalHopper.isHopperFull()) { + // if Stager is full + if (globalStager.getHasGP()) { globalIntake.setIntakeVelocity(0); } else { globalIntake.setIntakeVelocity(0.5); diff --git a/src/main/java/frc/robot/commands/intakeHopper.java b/src/main/java/frc/robot/commands/intakeHopper.java index 24307ec..d591e9f 100644 --- a/src/main/java/frc/robot/commands/intakeHopper.java +++ b/src/main/java/frc/robot/commands/intakeHopper.java @@ -6,15 +6,18 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Hopper; +import frc.robot.subsystems.Stager; public class intakeHopper extends Command { Hopper subHopper; + Stager subStager; /** Creates a new intakeHopper. */ - public intakeHopper(Hopper subHopper) { + public intakeHopper(Hopper subHopper, Stager subStager) { // Use addRequirements() here to declare subsystem dependencies. this.subHopper = subHopper; + this.subStager = subStager; } @@ -28,7 +31,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (subHopper.isHopperFull()) { + if (subStager.getHasGP()) { subHopper.setOrientationMotorNuetralOutput(); } else { subHopper.setOrientationMotorSpeed(.5); diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 60cc6a1..715f25a 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -14,12 +14,10 @@ public class Hopper extends SubsystemBase { /** Creates a new Hopper. */ TalonFX orientationMotor; DigitalInput isGPDetected; - DigitalInput isHopperFull; public Hopper() { orientationMotor = new TalonFX(RobotMap.mapHopper.ORIENTATION_MOTOR); isGPDetected = new DigitalInput(RobotMap.mapHopper.IS_GP_DETECTED_DIO); - isHopperFull = new DigitalInput(RobotMap.mapHopper.IS_HOPPER_FULL_DIO); } @@ -36,11 +34,6 @@ public boolean getGamePieceHopper() { return isGPDetected.get(); } - public boolean isHopperFull() { - - return isHopperFull.get(); - } - @Override public void periodic() {