diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 61a21b1..ce62b1f 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -4,6 +4,7 @@ package frc.robot.commands.states; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.constants.ConstMotion; @@ -12,6 +13,9 @@ public class Shooting extends Command { /** Creates a new Shooting. */ + + private final Timer intakeAgitationTimer = new Timer(); + public Shooting() { // Use addRequirements() here to declare subsystem dependencies. @@ -24,11 +28,24 @@ public void initialize() { RobotContainer.stateMachineInstance.setRobotState((StateMachine.RobotState.SHOOTING)); RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.TRANSFER_ROLLERS_SPEED); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.SERIALIZER_SHOOTING_SPEED); + intakeAgitationTimer.restart(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if (!RobotContainer.motionInstance.getIntakePivotAngle().isNear(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, + ConstMotion.INTAKE_PIVOT_TOLERANCE) + && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); + intakeAgitationTimer.restart(); + + } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); + intakeAgitationTimer.restart(); + } + } // Called once the command ends or is interrupted. @@ -38,7 +55,8 @@ public void end(boolean interrupted) { RobotContainer.rotorsInstance.setTransferRollersPercentOutput(ConstRotors.STOP); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.STOP); RobotContainer.motionInstance.setHoodPivotAngle(ConstMotion.HOOD_PIVOT_ANGLE_RETRACT); - + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); + intakeAgitationTimer.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index d367b3f..118cfc7 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -15,9 +15,12 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Time; /** Add your docs here. */ public class ConstMotion { + public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(1.0); + public static final Time INTAKE_PIVOT_AGITATION_TIME = Units.Seconds.of(1.0 / 3.0); public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle DEPLOY_INTAKE_PIVOT_ANGLE = Degrees.of(125); diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 39c82b0..edf68b8 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -29,7 +29,6 @@ public class Motion extends SubsystemBase { Angle lastDesiredIntakePivotAngle = Degrees.zero(); private boolean hoodAtPostion = false; - private boolean intakePivotAtPosition = false; public Motion() { intakePivot.getConfigurator().apply(ConstMotion.INTAKE_PIVOT_CONFIGURATION); @@ -80,17 +79,6 @@ public boolean isHoodPivotAtAngle(Angle tolerance) { return hoodAtPostion; } - public boolean isIntakePivotAtAngle(Angle tolerance) { - Angle lowerlim = lastDesiredIntakePivotAngle.minus(tolerance); - Angle upperlim = lastDesiredIntakePivotAngle.plus(tolerance); - - Angle intakePivotAngle = getIntakePivotAngle(); - - intakePivotAtPosition = intakePivotAngle.gte(lowerlim) - && intakePivotAngle.lte(upperlim); - return intakePivotAtPosition; - } - @Override public void periodic() { // This method will be called once per scheduler run