diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java index bd113f7..5216f98 100644 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/intake/IntakeConstants.java @@ -32,6 +32,8 @@ public static class FrontRollerConstants { public static final double INTAKE_SPEED = 34; + public static final double EJECT_SPEED = -34; + /** Maximum speed of the roller in rotations per second. */ public static final double MAXIMUM_SPEED = 67; } @@ -62,6 +64,8 @@ public static class BackRollerConstants { public static final double INTAKE_SPEED = 34; + public static final double EJECT_SPEED = -34; + /** Maximum speed of the roller in rotations per second. */ public static final double MAXIMUM_SPEED = 67; } diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java index 51a6da5..2b1f650 100644 --- a/src/main/java/frc/robot/intake/IntakeState.java +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -13,6 +13,8 @@ public record IntakeState( public static final IntakeState INTAKE = new IntakeState(FrontRollerConstants.INTAKE_SPEED, BackRollerConstants.INTAKE_SPEED); + public static final IntakeState EJECT = new IntakeState(FrontRollerConstants.EJECT_SPEED, BackRollerConstants.EJECT_SPEED); + public IntakeState { Objects.requireNonNull(frontRollerVelocityRotationsPerSecond); Objects.requireNonNull(backRollerVelocityRotationsPerSecond); diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 04405a8..51bc0cf 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -22,10 +22,10 @@ public record SuperstructureState( new SuperstructureState(ArmState.STOW, IntakeState.INTAKE, ShooterState.INTAKE); public static final SuperstructureState EJECT_POSITION = - new SuperstructureState(ArmState.EJECT, IntakeState.IDLE, ShooterState.IDLE); + new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.IDLE); public static final SuperstructureState EJECT = - new SuperstructureState(ArmState.EJECT, IntakeState.IDLE, ShooterState.EJECT); + new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.EJECT); public static final SuperstructureState SPEAKER_PULL = new SuperstructureState(ArmState.SPEAKER, IntakeState.IDLE, ShooterState.PULL);