diff --git a/src/main/java/frc/robot/commands/states/EjectingHopper.java b/src/main/java/frc/robot/commands/states/EjectingHopper.java index 7090431..91aea47 100644 --- a/src/main/java/frc/robot/commands/states/EjectingHopper.java +++ b/src/main/java/frc/robot/commands/states/EjectingHopper.java @@ -5,25 +5,46 @@ package frc.robot.commands.states; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.constants.ConstMotion; +import frc.robot.constants.ConstRotors; +import frc.robot.subsystems.StateMachine; +import frc.robot.*; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class EjectingHopper extends Command { +public class EjectingHopper extends Command + +{ /** Creates a new EjectingHopper. */ public EjectingHopper() { + + addRequirements(RobotContainer.stateMachineInstance); + // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + RobotContainer.stateMachineInstance.setRobotState(StateMachine.RobotState.EJECTING_HOPPER); + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY); + RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.REVERSE_SERIALIZER_ROLLERS_SPEED); + RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.REVERSE_TRANSFER_ROLLERS_SPEED); + RobotContainer.rotorsInstance.setTransferRollersPercentOutput(ConstRotors.REVERSE_INTAKE_ROLLERS_SPEED); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.STOP); + RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.STOP); + RobotContainer.rotorsInstance.setTransferRollersPercentOutput(ConstRotors.STOP); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java index 02bdcdf..86c5c38 100644 --- a/src/main/java/frc/robot/commands/states/Intaking.java +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -18,13 +18,13 @@ public Intaking() { @Override public void initialize() { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY); + RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.INTAKE_ROLLERS_SPEED); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.INTAKE_ROLLERS_SPEED); - } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/constants/ConstRotors.java b/src/main/java/frc/robot/constants/ConstRotors.java index 2200f13..2959fa7 100644 --- a/src/main/java/frc/robot/constants/ConstRotors.java +++ b/src/main/java/frc/robot/constants/ConstRotors.java @@ -28,6 +28,10 @@ public class ConstRotors { public static final double SERIALIZER_SHOOTING_SPEED = 0.5; public static final double STOP = 0; public static final double REVERSE_SHOOTING_SPEED = -1; + public static final double REVERSE_SERIALIZER_ROLLERS_SPEED = -0.5; + public static final double REVERSE_INTAKE_ROLLERS_SPEED = -0.5; + public static final double REVERSE_TRANSFER_ROLLERS_SPEED = -0.5; + static { // SHOOTER_TRANSFER_EAST_CONFIGURATION.MotorOutput.NeutralMode =