diff --git a/src/main/java/frc/robot/commands/states/EjectingHopper.java b/src/main/java/frc/robot/commands/states/EjectingHopper.java index 91aea47..5fa1ee1 100644 --- a/src/main/java/frc/robot/commands/states/EjectingHopper.java +++ b/src/main/java/frc/robot/commands/states/EjectingHopper.java @@ -9,7 +9,6 @@ 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 @@ -27,7 +26,7 @@ public EjectingHopper() { @Override public void initialize() { RobotContainer.stateMachineInstance.setRobotState(StateMachine.RobotState.EJECTING_HOPPER); - RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY); + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.REVERSE_SERIALIZER_ROLLERS_SPEED); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.REVERSE_TRANSFER_ROLLERS_SPEED); RobotContainer.rotorsInstance.setTransferRollersPercentOutput(ConstRotors.REVERSE_INTAKE_ROLLERS_SPEED); diff --git a/src/main/java/frc/robot/commands/states/Intaking.java b/src/main/java/frc/robot/commands/states/Intaking.java index 86c5c38..24544de 100644 --- a/src/main/java/frc/robot/commands/states/Intaking.java +++ b/src/main/java/frc/robot/commands/states/Intaking.java @@ -8,17 +8,20 @@ import frc.robot.RobotContainer; import frc.robot.constants.ConstMotion; import frc.robot.constants.ConstRotors; +import frc.robot.subsystems.StateMachine; public class Intaking extends Command { /** Creates a new Intaking. */ public Intaking() { + addRequirements(RobotContainer.stateMachineInstance); } // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_DEPLOY); + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.INTAKE_ROLLERS_SPEED); + RobotContainer.stateMachineInstance.setRobotState(StateMachine.RobotState.INTAKING); } diff --git a/src/main/java/frc/robot/commands/states/RetractIntake.java b/src/main/java/frc/robot/commands/states/RetractIntake.java index 75f0249..c17e3e0 100644 --- a/src/main/java/frc/robot/commands/states/RetractIntake.java +++ b/src/main/java/frc/robot/commands/states/RetractIntake.java @@ -20,8 +20,8 @@ public RetractIntake() { // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.INTAKE_PIVOT_RETRACT); RobotContainer.rotorsInstance.setIntakeRollersPercentOutput(ConstRotors.STOP); + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); RobotContainer.stateMachineInstance.setRobotState(StateMachine.RobotState.RETRACT_INTAKE); } diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 534bd6f..c011ea6 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -20,8 +20,8 @@ public class ConstMotion { public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); - public static final Angle INTAKE_PIVOT_DEPLOY = Degrees.of(125); - public static final Angle INTAKE_PIVOT_RETRACT = Degrees.of(0); + public static final Angle DEPLOY_INTAKE_PIVOT_ANGLE = Degrees.of(125); + public static final Angle RETRACT_INTAKE_PIVOT_ANGLE = Degrees.of(0); public static final Angle HOOD_PIVOT_ANGLE = Degrees.of(14); public static final double STOP = 0; public static final Angle HOOD_NONE_ANGLE = Degrees.of(0); @@ -32,8 +32,8 @@ public class ConstMotion { INTAKE_PIVOT_CONFIGURATION.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = INTAKE_PIVOT_DEPLOY.in(Rotations); - INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = INTAKE_PIVOT_RETRACT.in(Rotations); + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ForwardSoftLimitThreshold = DEPLOY_INTAKE_PIVOT_ANGLE.in(Rotations); + INTAKE_PIVOT_CONFIGURATION.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RETRACT_INTAKE_PIVOT_ANGLE.in(Rotations); INTAKE_PIVOT_CONFIGURATION.Slot0.GravityType = GravityTypeValue.Arm_Cosine; INTAKE_PIVOT_CONFIGURATION.Slot0.kS = 0.3; INTAKE_PIVOT_CONFIGURATION.Slot0.kP = 40;